From 7612fa40ed6bf24e5bb6f2d58cf423471b45ab5c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 30 May 2019 08:53:29 +1000 Subject: [PATCH] EKF: Don't start using GPS for height until all validity checks have passed Consolidate intermittent data checks, improve variable and clarify usage. --- EKF/control.cpp | 29 ++++++++++------------------- EKF/ekf.h | 5 +++-- EKF/gps_checks.cpp | 4 ++-- 3 files changed, 15 insertions(+), 23 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index e208706666..d0f83459c5 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -80,12 +80,12 @@ void Ekf::controlFusionModes() } - // check faultiness (before pop_first_older_than) to see if we can change back to original height sensor + // check for intermittent data (before pop_first_older_than) const baroSample &baro_init = _baro_buffer.get_newest(); _baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); const gpsSample &gps_init = _gps_buffer.get_newest(); - _gps_hgt_faulty = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); + _gps_hgt_intermittent = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); // check for arrival of new sensor data at the fusion time horizon _gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); @@ -115,7 +115,7 @@ void Ekf::controlFusionModes() Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2; } - + // We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon. // This means we stop looking for new data until the old data has been fused. if (!_flow_data_ready) { @@ -697,7 +697,7 @@ void Ekf::controlGpsFusion() _control_status.flags.gps = false; ECL_WARN("EKF GPS data stopped"); } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) { - // Handle the case where we are fusing another position source along GPS, + // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal _control_status.flags.gps = false; ECL_WARN("EKF GPS data stopped, using only EV or OF"); @@ -754,7 +754,6 @@ void Ekf::controlHeightSensorTimeouts() if (_control_status.flags.baro_hgt) { // check if GPS height is available const gpsSample &gps_init = _gps_buffer.get_newest(); - bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); const baroSample &baro_init = _baro_buffer.get_newest(); @@ -764,10 +763,10 @@ void Ekf::controlHeightSensorTimeouts() bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION); // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data - bool reset_to_gps = gps_hgt_available && gps_hgt_accurate && !_gps_hgt_faulty && !prev_bad_vert_accel; + bool reset_to_gps = !_gps_hgt_intermittent && gps_hgt_accurate && !prev_bad_vert_accel; // reset to GPS if GPS data is available and there is no Baro data - reset_to_gps = reset_to_gps || (gps_hgt_available && !baro_hgt_available); + reset_to_gps = reset_to_gps || (!_gps_hgt_intermittent && !baro_hgt_available); // reset to Baro if we are not doing a GPS reset and baro data is available bool reset_to_baro = !reset_to_gps && baro_hgt_available; @@ -776,9 +775,6 @@ void Ekf::controlHeightSensorTimeouts() // set height sensor health _baro_hgt_faulty = true; - // declare the GPS height healthy - _gps_hgt_faulty = false; - // reset the height mode setControlGPSHeight(); @@ -809,7 +805,6 @@ void Ekf::controlHeightSensorTimeouts() if (_control_status.flags.gps_hgt) { // check if GPS height is available const gpsSample &gps_init = _gps_buffer.get_newest(); - bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); // check the baro height source for consistency and freshness @@ -822,14 +817,13 @@ void Ekf::controlHeightSensorTimeouts() bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate; // if GPS height is unavailable and baro data is available, reset height to baro - reset_to_baro = reset_to_baro || (!gps_hgt_available && baro_data_fresh); + reset_to_baro = reset_to_baro || (_gps_hgt_intermittent && baro_data_fresh); // if we cannot switch to baro and GPS data is available, reset height to GPS - bool reset_to_gps = !reset_to_baro && gps_hgt_available; + bool reset_to_gps = !reset_to_baro && !_gps_hgt_intermittent; if (reset_to_baro) { // set height sensor health - _gps_hgt_faulty = true; _baro_hgt_faulty = false; // reset the height mode @@ -840,9 +834,6 @@ void Ekf::controlHeightSensorTimeouts() ECL_WARN("EKF gps hgt timeout - reset to baro"); } else if (reset_to_gps) { - // set height sensor health - _gps_hgt_faulty = false; - // reset the height mode setControlGPSHeight(); @@ -999,7 +990,7 @@ void Ekf::controlHeightFusion() } } - } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) { + } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) { // switch to gps if there was a reset to gps _fuse_height = true; @@ -1063,7 +1054,7 @@ void Ekf::controlHeightFusion() } } - } else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_faulty) { + } else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) { setControlGPSHeight(); _fuse_height = true; diff --git a/EKF/ekf.h b/EKF/ekf.h index 8815cd8dd3..e2bff69676 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -415,6 +415,7 @@ private: uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks float _gps_error_norm{1.0f}; ///< normalised gps error uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time + bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed // Variables used to publish the WGS-84 location of the EKF local NED origin uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec) @@ -463,9 +464,9 @@ private: float _dt_last_range_update_filt_us{0.0f}; ///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec) bool _hagl_valid{false}; ///< true when the height above ground estimate is valid - // height sensor fault status + // height sensor status bool _baro_hgt_faulty{false}; ///< true if valid baro data is unavailable for use - bool _gps_hgt_faulty{false}; ///< true if valid gps height data is unavailable for use + bool _gps_hgt_intermittent{false}; ///< true if gps height into the buffer is intermittent bool _rng_hgt_faulty{false}; ///< true if valid range finder height data is unavailable for use int _primary_hgt_source{VDIST_SENSOR_BARO}; ///< specifies primary source of height data diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index e70aca5981..bbd3d09a3f 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -59,8 +59,8 @@ bool Ekf::collect_gps(const gps_message &gps) { // Run GPS checks always - bool gps_checks_pass = gps_is_good(gps); - if (!_NED_origin_initialised && gps_checks_pass) { + _gps_checks_passed = gps_is_good(gps); + if (!_NED_origin_initialised && _gps_checks_passed) { // If we have good GPS data set the origin's WGS-84 position to the last gps fix double lat = gps.lat / 1.0e7; double lon = gps.lon / 1.0e7;