diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 264c826738..dc6d36bcb5 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -126,7 +126,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired()) { + if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::BARO)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); @@ -142,10 +142,13 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) aid_src.time_last_fuse = imu_sample.time_us; } else if (is_fusion_failing) { - // Some other height source is still working ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); stopBaroHgtFusion(); - _baro_hgt_faulty = true; + + if (isRecent(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max)) { + // Some other height source is still working + _baro_hgt_faulty = true; + } } } else { diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index e7b6f13f11..440c0fe7ac 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -142,7 +142,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired() && quality_sufficient) { + if (isHeightResetRequired() && quality_sufficient && (_height_sensor_ref == HeightSensor::EV)) { // All height sources are failing ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index c141d415ee..91a8685ceb 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -100,12 +100,12 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired()) { + if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::GNSS)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var); + resetVerticalPositionTo(aid_src.observation, measurement_var); bias_est.setBias(_state.pos(2) + measurement); aid_src.time_last_fuse = _time_delayed_us; 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 6c33d4db63..0aafe8a1b0 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 @@ -173,7 +173,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired() && _control_status.flags.rng_hgt) { + if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); diff --git a/src/modules/ekf2/test/sensor_simulator/gps.h b/src/modules/ekf2/test/sensor_simulator/gps.h index f962cc9bce..e8ff206916 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.h +++ b/src/modules/ekf2/test/sensor_simulator/gps.h @@ -66,6 +66,7 @@ public: void setPdop(const float pdop); gnssSample getDefaultGpsData(); + const gnssSample &getData() const { return _gps_data; } private: void send(uint64_t time) override; diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index c80939f1ba..b322a28182 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -261,6 +261,93 @@ TEST_F(EkfHeightFusionTest, gpsRefFailOver) EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::UNKNOWN); } +TEST_F(EkfHeightFusionTest, gpsRefAllHgtFailReset) +{ + // GIVEN: EKF that fuses GNSS (reference) and baro + _sensor_simulator.startBaro(); + _sensor_simulator.startGps(); + _ekf_wrapper.setGpsHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + + _sensor_simulator.runSeconds(11); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS); + + const Vector3f previous_position = _ekf->getPosition(); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: + const float gnss_height_step = 10.f; + _sensor_simulator._gps.stepHeightByMeters(gnss_height_step); + + const float baro_height_step = 5.f; + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_height_step); + _sensor_simulator.runSeconds(15); + + // THEN: then the fusion of both sensors starts to fail and the height is reset to the + // reference sensor (GNSS) + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + + const Vector3f new_position = _ekf->getPosition(); + EXPECT_NEAR(new_position(2), previous_position(2) - gnss_height_step, 0.2f); + + // Also check the reset counters to make sure the reset logic triggered + reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(1)); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); +} + +TEST_F(EkfHeightFusionTest, baroRefAllHgtFailReset) +{ + // GIVEN: EKF that fuses GNSS and baro (reference) + _sensor_simulator.startBaro(); + _sensor_simulator.startGps(); + _ekf_wrapper.setBaroHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + + _sensor_simulator.runSeconds(11); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); + + const Vector3f previous_position = _ekf->getPosition(); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: + const float gnss_height_step = 10.f; + _sensor_simulator._gps.stepHeightByMeters(gnss_height_step); + + const float baro_height_step = 5.f; + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_height_step); + _sensor_simulator.runSeconds(20); + + // THEN: then the fusion of both sensors starts to fail and the height is reset to the + // reference sensor (baro) + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + + const Vector3f new_position = _ekf->getPosition(); + EXPECT_NEAR(new_position(2), previous_position(2) - baro_height_step, 0.2f); + + // Also check the reset counters to make sure the reset logic triggered + reset_logging_checker.capturePostResetState(); + + // The velocity does not reset as baro only provides height measurement + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); + + // The height resets twice in a row as the baro innovation is not corrected after a height + // reset and triggers a new reset at the next iteration + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2)); +} + TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) { _sensor_simulator.startBaro();