From 16d1e15b5124a61468dfcbfd1e6c258f3199f3d4 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 15 Oct 2019 14:14:46 +0200 Subject: [PATCH] addressed review comments Signed-off-by: RomanBapst --- EKF/control.cpp | 2 +- EKF/ekf.h | 8 ++++++-- EKF/range_finder_checks.cpp | 24 ++++++++++-------------- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 9203bd832f..b0349deaa7 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1176,7 +1176,7 @@ void Ekf::checkRangeAidSuitability() || _control_status.flags.opt_flow; if (_control_status.flags.in_air - && !_rng_hgt_faulty + && _rng_hgt_valid && isTerrainEstimateValid() && horz_vel_valid) { // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching diff --git a/EKF/ekf.h b/EKF/ekf.h index 0fe2ed7342..ddf6cdeb92 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -457,7 +457,6 @@ private: float _sin_tilt_rng{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis float _cos_tilt_rng{0.0f}; ///< cosine of the range finder tilt rotation about the Y body axis float _R_rng_to_earth_2_2{0.0f}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame - bool _range_data_continuous{false}; ///< true when we are receiving range finder data faster than a 2Hz average 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 @@ -648,9 +647,12 @@ private: void checkRangeAidSuitability(); bool isRangeAidSuitable() { return _is_range_aid_suitable; } - // check for "stuck" range finder measurements when rng was not valid for certain period + // update _rng_hgt_valid, which indicates if the current range sample has passed validity checks void updateRangeDataValidity(); + // check for "stuck" range finder measurements when rng was not valid for certain period + void updateRangeDataStuck(); + // return the square of two floating point numbers - used in auto coded sections static constexpr float sq(float var) { return var * var; } @@ -700,6 +702,8 @@ private: // check that the range finder data is continuous void updateRangeDataContinuity(); + bool isRangeDataContinuous() { return _dt_last_range_update_filt_us < 2e6f; } + // Increase the yaw error variance of the quaternions // Argument is additional yaw variance in rad**2 void increaseQuatYawErrVariance(float yaw_variance); diff --git a/EKF/range_finder_checks.cpp b/EKF/range_finder_checks.cpp index 0430da3a56..a7eaeb1554 100644 --- a/EKF/range_finder_checks.cpp +++ b/EKF/range_finder_checks.cpp @@ -55,13 +55,6 @@ void Ekf::updateRangeDataContinuity() (_imu_sample_delayed.time_us - _range_sample_delayed.time_us); _dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f); - - if (_dt_last_range_update_filt_us < 2e6f) { - _range_data_continuous = true; - - } else { - _range_data_continuous = false; - } } void Ekf::updateRangeDataValidity() @@ -69,13 +62,13 @@ void Ekf::updateRangeDataValidity() updateRangeDataContinuity(); // check if out of date - if ((_imu_sample_delayed.time_us - _range_sample_delayed .time_us) > 2 * RNG_MAX_INTERVAL) { + if ((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) > 2 * RNG_MAX_INTERVAL) { _rng_hgt_valid = false; return; } // Don't allow faulty flag to clear unless range data is continuous - if (!_rng_hgt_valid && !_range_data_continuous) { + if (!_rng_hgt_valid && !isRangeDataContinuous()) { return; } @@ -87,10 +80,7 @@ void Ekf::updateRangeDataValidity() if (_range_sample_delayed.quality == 0) { _time_bad_rng_signal_quality = _imu_sample_delayed.time_us; _rng_hgt_valid = false; - } else if (_time_bad_rng_signal_quality > 0 && _imu_sample_delayed.time_us - _time_bad_rng_signal_quality > RNG_BAD_SIG_HYST) { - _time_bad_rng_signal_quality = 0; - _rng_hgt_valid = true; - } else if (_time_bad_rng_signal_quality == 0) { + } else if (_imu_sample_delayed.time_us - _time_bad_rng_signal_quality > RNG_BAD_SIG_HYST) { _rng_hgt_valid = true; } @@ -115,6 +105,13 @@ void Ekf::updateRangeDataValidity() } } + updateRangeDataStuck(); + + _rng_hgt_valid = !_control_status.flags.rng_stuck; +} + +void Ekf::updateRangeDataStuck() +{ // 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) && @@ -137,7 +134,6 @@ void Ekf::updateRangeDataValidity() } _control_status.flags.rng_stuck = true; - _rng_hgt_valid = false; } } else {