diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index 58b1353573..45d917cb7e 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -39,7 +39,7 @@ using namespace matrix; -void RangeFinderConsistencyCheck::initMiniKF(float var_z, float var_terrain, float z, float dist_bottom) +void RangeFinderConsistencyCheck::init(float var_z, float var_terrain, float z, float dist_bottom) { _R.setZero(); _A.setIdentity(); @@ -54,7 +54,7 @@ void RangeFinderConsistencyCheck::initMiniKF(float var_z, float var_terrain, flo _state = KinematicState::UNKNOWN; } -void RangeFinderConsistencyCheck::UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, +void RangeFinderConsistencyCheck::update(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, uint64_t time_us) { const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; @@ -112,6 +112,8 @@ void RangeFinderConsistencyCheck::UpdateMiniKF(float z, float z_var, float vz, f _state = KinematicState::INCONSISTENT; } + } else { + _test_ratio_lpf.update(test_ratio); } } @@ -125,8 +127,8 @@ void RangeFinderConsistencyCheck::run(const float z, const float vz, if (!_initialized || current_posD_reset_count != _last_posD_reset_count) { _last_posD_reset_count = current_posD_reset_count; const float terrain_var = P(estimator::State::terrain.idx, estimator::State::terrain.idx); - initMiniKF(z_var, terrain_var, z, dist_bottom); + init(z_var, terrain_var, z, dist_bottom); } - UpdateMiniKF(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us); + update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us); } diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp index 5997984da8..c1ffb4eadd 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp @@ -62,10 +62,10 @@ public: bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; } bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; } - void UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, + void update(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, uint64_t time_us); - void initMiniKF(float var_z, float var_terrain, float z, float dist_bottom); - void stopMiniKF() + void init(float var_z, float var_terrain, float z, float dist_bottom); + void stop() { if (_initialized) { if (_state == KinematicState::CONSISTENT) { diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 78acdd914c..66c03f24b5 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -80,7 +80,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (horizontal_motion) { if (_rng_consistency_check.isRunning()) { - _rng_consistency_check.stopMiniKF(); + _rng_consistency_check.stop(); } } else if (vertical_motion) { @@ -103,7 +103,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _range_sensor.setValidity(true); // bypass the checks } else { - _rng_consistency_check.stopMiniKF(); + _rng_consistency_check.stop(); } } }