From 2d978beefbc010d962034a0f0d588cbf46a063f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:16:44 +0200 Subject: [PATCH 1/5] Compile fixes --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 6eceb21f8e..ffebcd4776 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2307,7 +2307,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(states[i])) { err_report->statesNaN = true; - ekf_debug("states NaN: i: %u val: %f", i, states[i]); + ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); err = true; goto out; } // state matrix diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1c943137a1..fdb5dd5883 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1213,8 +1213,8 @@ FixedwingEstimator::task_main() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; } if (_local_pos.v_xy_valid) { @@ -1227,7 +1227,6 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; From 18a932fe948b06b87d65f5d13102274e5683fab6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:35:22 +0200 Subject: [PATCH 2/5] Better fake / simulation values --- src/drivers/gps/gps.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f5..01be80dcec 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -276,14 +276,14 @@ GPS::task_main() _report.timestamp_position = hrt_absolute_time(); _report.lat = (int32_t)47.378301e7f; _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; + _report.alt = (int32_t)1200e3f; _report.timestamp_variance = hrt_absolute_time(); _report.s_variance_m_s = 10.0f; _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 3.0f; - _report.epv_m = 7.0f; + _report.eph_m = 0.9f; + _report.epv_m = 1.8f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; From 6612cdab856674c4cbec53863c470cb4e96a9f7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:36:09 +0200 Subject: [PATCH 3/5] Let commander be less pedantic about positioning data --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3527117341..148a0aafa2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 50000 /**< consider the local or global position estimate invalid after 50ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 From 925430796ed56e97bf2310423361a8e6e29e350b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:37:26 +0200 Subject: [PATCH 4/5] Reworked how we deal with altitudes --- .../fw_att_pos_estimator_main.cpp | 79 ++++++++++++------- .../fw_att_pos_estimator_params.c | 11 +++ 2 files changed, 62 insertions(+), 28 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index fdb5dd5883..a02b2c34fb 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -191,6 +191,7 @@ private: perf_counter_t _perf_airspeed; ///yawVarScale = 1.0f; @@ -568,18 +574,10 @@ FixedwingEstimator::task_main() #endif bool newDataGps = false; + bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; - // Reset relevant structs - _gps = {}; - - #ifndef SENSOR_COMBINED_SUB - _gyro = {}; - #else - _sensor_combined = {}; - #endif - while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -611,6 +609,7 @@ FixedwingEstimator::task_main() if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ + bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; @@ -618,6 +617,12 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); + /* Reset baro reference if switching to HIL */ + if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + _baro_init = false; + _gps_initialized = false; + } + /** * PART ONE: COLLECT ALL DATA **/ @@ -813,10 +818,17 @@ FixedwingEstimator::task_main() if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _ekf->baroHgt = _baro.altitude - _baro_ref; + _ekf->baroHgt = _baro.altitude; - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + if (!_baro_init) { + _baro_ref = _baro.altitude; + _baro_init = true; + warnx("ALT REF INIT"); + } + + newHgtData = true; + } else { + newHgtData = false; } #ifndef SENSOR_COMBINED_SUB @@ -904,9 +916,9 @@ FixedwingEstimator::task_main() } } - // XXX trap for testing + // warn on fatal resets if (check == 1) { - errx(1, "NUMERIC ERROR IN FILTER"); + warnx("NUMERIC ERROR IN FILTER"); } // If non-zero, we got a filter reset @@ -960,7 +972,7 @@ FixedwingEstimator::task_main() // struct home_position_s home; // orb_copy(ORB_ID(home_position), _home_sub, &home); - if (!_gps_initialized && _gps.fix_type > 2) { + if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -968,26 +980,37 @@ FixedwingEstimator::task_main() // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; - float alt = _gps.alt / 1e3f; + float gps_alt = _gps.alt / 1e3f; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_gps_offset = gps_alt - _baro.altitude; + _ekf->baroHgt = _baro.altitude; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + + // Set up position variables correctly + _ekf->GPSstatus = _gps.fix_type; + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); // Initialize projection _local_pos.ref_lat = _gps.lat; _local_pos.ref_lon = _gps.alt; - _local_pos.ref_alt = alt; + _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; - // Store - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref = _baro.altitude; - _ekf->baroHgt = _baro.altitude - _baro_ref; - _baro_gps_offset = _baro_ref - _local_pos.ref_alt; - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt); - warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt, + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); _gps_initialized = true; @@ -1089,9 +1112,9 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newAdsData && _ekf->statesInitialised) { + if (newHgtData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c index cfcd99858e..d2c6e1f15e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c @@ -258,3 +258,14 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); */ PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); +/** + * Threshold for filter initialization. + * + * If the standard deviation of the GPS position estimate is below this threshold + * in meters, the filter will initialize. + * + * @min 0.3 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f); From 6ab878c0218f26e5fa71053b75d7075b594b937e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:38:37 +0200 Subject: [PATCH 5/5] Emit the local position against the GPS reference - this means it can jump. --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a02b2c34fb..a82e8d7044 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1203,7 +1203,7 @@ FixedwingEstimator::task_main() _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; - _local_pos.z = _ekf->states[9]; + _local_pos.z = _ekf->states[9] + _baro_gps_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5];