From 17d40478bb8485a70e15f69609a535523dbb4516 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 13 Jul 2018 11:42:55 +1000 Subject: [PATCH] EKF: Rework range height validity checking Eliminate race condition caused by checking for data freshness using time stamps from buffer push instead than buffer pop events. Consistent use of range data ready and range data fault flags. This achieved by ensuring that _rng_hgt_faulty is set to true for all range data faults, not just data freshness. Include range data validity requirement in rangeAidConditionsMet() check. --- EKF/control.cpp | 50 ++++++++++++++++++++++----------------- EKF/ekf.h | 2 +- EKF/terrain_estimator.cpp | 2 +- 3 files changed, 30 insertions(+), 24 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index df00e4f0a0..3b3559c952 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -90,9 +90,6 @@ void Ekf::controlFusionModes() const gpsSample &gps_init = _gps_buffer.get_newest(); _gps_hgt_faulty = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); - const rangeSample &rng_init = _range_buffer.get_newest(); - _rng_hgt_faulty = !((_time_last_imu - rng_init.time_us) < 2 * RNG_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); _mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed); @@ -115,6 +112,13 @@ void Ekf::controlFusionModes() if (_range_data_ready) { checkRangeDataValidity(); + + if (_range_data_ready && !_rng_hgt_faulty) { + // correct the range data for position offset relative to the IMU + Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; + 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. @@ -907,13 +911,6 @@ void Ekf::controlHeightFusion() { // set control flags for the desired primary height source - if (_range_data_ready) { - // correct the range data for position offset relative to the IMU - Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; - Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2; - } - rangeAidConditionsMet(); _range_aid_mode_selected = (_params.range_aid == 1) && _range_aid_mode_enabled; @@ -1068,7 +1065,7 @@ void Ekf::controlHeightFusion() } if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt - && !_range_data_ready) { + && (!_range_data_ready || _rng_hgt_faulty)) { // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements // and are on the ground, then synthesise a measurement at the expected on ground value @@ -1088,11 +1085,12 @@ void Ekf::rangeAidConditionsMet() { // if the parameter for range aid is enabled we allow to switch from using the primary height source to using range finder as height source // under the following conditions - // 1) we are not further than max_hagl_for_range_aid away from the ground - // 2) our ground speed is not higher than max_vel_for_range_aid - // 3) Our terrain estimate is stable (needs better checks) - // 4) We are in-air - if (_control_status.flags.in_air) { + // 1) Vehicle is in-air + // 2) Range data is valid + // 3) Vehicle is no further than max_hagl_for_range_aid away from the ground + // 4) Ground speed is not higher than max_vel_for_range_aid + // 5) Terrain estimate is stable (needs better checks) + if (_control_status.flags.in_air && !_rng_hgt_faulty) { // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching bool can_use_range_finder; if (_range_aid_mode_enabled) { @@ -1142,9 +1140,18 @@ void Ekf::rangeAidConditionsMet() void Ekf::checkRangeDataValidity() { + // reset fault status + _rng_hgt_faulty = false; + + // check if out of date + if ((_imu_sample_delayed.time_us - _range_sample_delayed .time_us) > 2 * RNG_MAX_INTERVAL) { + _rng_hgt_faulty = true; + return; + } + // Check if excessively tilted if (_R_rng_to_earth_2_2 < _params.range_cos_max_tilt) { - _range_data_ready = false; + _rng_hgt_faulty = true; return; } @@ -1152,7 +1159,7 @@ void Ekf::checkRangeDataValidity() if ((_range_sample_delayed.rng > _rng_valid_max_val) || (_range_sample_delayed.rng < _rng_valid_min_val)) { if (_control_status.flags.in_air) { - _range_data_ready = false; + _rng_hgt_faulty = true; return; } else { // Range finders can fail to provide valid readings when resting on the ground @@ -1163,13 +1170,11 @@ void Ekf::checkRangeDataValidity() } } - // Check for "stuck" range finder measurements when rng was not valid for certain period + // Check for "stuck" range finder measurements when range was not valid for certain period // This handles a failure mode observed with some lidar sensors if (_range_sample_delayed.time_us - _time_last_rng_ready > (uint64_t)10e6 && _control_status.flags.in_air) { - _control_status.flags.rng_stuck = true; - // require a variance of rangefinder values to check for "stuck" measurements if (_rng_stuck_max_val - _rng_stuck_min_val > 1.0f) { _time_last_rng_ready = _range_sample_delayed.time_us; @@ -1186,7 +1191,8 @@ void Ekf::checkRangeDataValidity() _rng_stuck_min_val = _range_sample_delayed.rng; } - _range_data_ready = false; + _control_status.flags.rng_stuck = true; + _rng_hgt_faulty = true; } } else { diff --git a/EKF/ekf.h b/EKF/ekf.h index ec39810748..7b17449979 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -431,7 +431,7 @@ private: // height sensor fault 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 _rng_hgt_faulty{false}; ///< true if valid rnage finder height data is unavailable for use + 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 // imu fault status diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 89c57ae999..b080093205 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -94,7 +94,7 @@ void Ekf::runTerrainEstimator() _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); // Fuse range finder data if available - if (_range_data_ready && !_control_status.flags.rng_stuck) { + if (_range_data_ready && !_rng_hgt_faulty) { fuseHagl(); // update range sensor angle parameters in case they have changed