diff --git a/EKF/common.h b/EKF/common.h index 5954d619b0..3eadc087fc 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -433,6 +433,7 @@ union filter_control_status_u { uint32_t mag_fault : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active + uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index c706dfec9f..67439bd42c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1033,14 +1033,14 @@ void Ekf::checkForStuckRange() { if (_range_data_ready && _range_sample_delayed.time_us - _time_last_rng_ready > (uint64_t)10e6 && _control_status.flags.in_air) { - _rng_stuck = true; + _control_status.flags.rng_stuck = true; //require a variance of rangefinder values to check for "stuck" measurements if (_rng_check_max_val - _rng_check_min_val > 1.0f) { _time_last_rng_ready = _range_sample_delayed.time_us; _rng_check_min_val = 0.0f; _rng_check_max_val = 0.0f; - _rng_stuck = false; + _control_status.flags.rng_stuck = false; } else { if (_range_sample_delayed.rng > _rng_check_max_val) { diff --git a/EKF/ekf.h b/EKF/ekf.h index 33ad1a07d9..6e223a6bfe 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -428,7 +428,6 @@ private: bool _in_range_aid_mode{false}; ///< true when range finder is to be used as the height reference instead of the primary height sensor // variables used to check for "stuck" rng data - bool _rng_stuck{false}; ///< true when rng data wasn't ready for more than 10s and new rng values haven't changed enough float _rng_check_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck float _rng_check_max_val{0.0f}; ///< maximum value for new rng measurement when being stuck diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 4f18536e48..b54fd9c6ff 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -93,7 +93,7 @@ void Ekf::runTerrainEstimator() _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); // Fuse range finder data if available - if (_range_data_ready && !_rng_stuck) { + if (_range_data_ready && !_control_status.flags.rng_stuck) { fuseHagl(); // update range sensor angle parameters in case they have changed @@ -158,7 +158,7 @@ void Ekf::fuseHagl() // return true if the terrain estimate is valid bool Ekf::get_terrain_valid() { - if (_terrain_initialised && _range_data_continuous && !_rng_stuck && + if (_terrain_initialised && _range_data_continuous && !_control_status.flags.rng_stuck && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) { return true;