From aa00bdcd5d8e348b1b812972765018d9d312abd2 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Fri, 7 Mar 2025 17:22:16 +0100 Subject: [PATCH] also run range consistency check while landed --- .../range_finder/range_height_control.cpp | 24 +++++++++---------- src/modules/ekf2/test/test_EKF_flow.cpp | 2 +- .../ekf2/test/test_EKF_height_fusion.cpp | 2 +- 3 files changed, 13 insertions(+), 15 deletions(-) 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 148b598c90..ac9b535423 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 @@ -66,23 +66,21 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - if (_control_status.flags.in_air) { - const float dist_var = getRngVar(); - _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); + const float dist_var = getRngVar(); + _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); - const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f); + const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f); - if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) { - _rng_consistency_check.reset(); - } - - _rng_consistency_check.horizontal_motion = updated_horizontal_motion; - const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); - const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); - _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), - dist_var, imu_sample.time_us); + if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) { + _rng_consistency_check.reset(); } + _rng_consistency_check.horizontal_motion = updated_horizontal_motion; + const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); + const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); + _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), + dist_var, imu_sample.time_us); + } else { // If we are supposed to be using range finder data but have bad range measurements // and are on the ground, then synthesise a measurement at the expected on ground value diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 7e00703971..1bf1fd7849 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -171,7 +171,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround) _ekf_wrapper.enableFlowFusion(); _sensor_simulator.startFlow(); _sensor_simulator.startRangeFinder(); - _sensor_simulator.runSeconds(1.0); + _sensor_simulator.runSeconds(2.0); // THEN: estimated velocity should match simulated velocity const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index d39dc72a99..27b98c9ad5 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -250,7 +250,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver) EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS); _sensor_simulator.stopGps(); - _sensor_simulator.runSeconds(10); + _sensor_simulator.runSeconds(14); EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE); _sensor_simulator.stopRangeFinder();