From 0d874cf00ac8945a76a6851a17682ffc6ab500e3 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 9 Dec 2021 13:53:10 +0100 Subject: [PATCH] ekf2: move aiding start details in dedicated function When an aiding source needs to be started, simply call the corresponding starting function and any required reset is handled in there. --- src/modules/ekf2/EKF/control.cpp | 49 +++++-------------- src/modules/ekf2/EKF/ekf.h | 5 ++ src/modules/ekf2/EKF/ekf_helper.cpp | 66 +++++++++++++++++++++----- src/modules/ekf2/test/test_EKF_gps.cpp | 1 + 4 files changed, 73 insertions(+), 48 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index eaa4b2da0a..b0a6c7991e 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -813,23 +813,14 @@ void Ekf::controlHeightFusion() // FALLTHROUGH case VDIST_SENSOR_BARO: - if (do_range_aid && _range_sensor.isDataHealthy()) { - setControlRangeHeight(); - - // we have just switched to using range finder, calculate height sensor offset such that current - // measurement matches our current height estimate - if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - _hgt_sensor_offset = _terrain_vpos; + if (do_range_aid) { + if (!_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) { + startRngAidHgtFusion(); } - } else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) { - startBaroHgtFusion(); - - } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) { - // we have just switched to using gps height, calculate height sensor offset such that current - // measurement matches our current height estimate - if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) { - _hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2); + } else { + if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty) { + startBaroHgtFusion(); } } @@ -858,49 +849,34 @@ void Ekf::controlHeightFusion() break; case VDIST_SENSOR_GPS: - // NOTE: emergency fallback due to extended loss of currently selected sensor data or failure // to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts. // Do switching between GPS and rangefinder if using range finder as a height source when close // to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers. if (do_range_aid) { if (!_control_status_prev.flags.rng_hgt && _range_sensor.isDataHealthy()) { - setControlRangeHeight(); - - // we have just switched to using range finder, calculate height sensor offset such that current - // measurement matches our current height estimate - _hgt_sensor_offset = _terrain_vpos; + startRngAidHgtFusion(); } } else { - if (_control_status_prev.flags.rng_hgt) { - // must stop using range finder so find another sensor now + if (!_control_status.flags.gps_hgt) { if (!_gps_hgt_intermittent && _gps_checks_passed) { - // GPS quality OK + // In fallback mode and GPS has recovered so start using it startGpsHgtFusion(); - } else if (!_baro_hgt_faulty) { + } else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty) { // Use baro as a fallback startBaroHgtFusion(); } - - } else if (_control_status.flags.baro_hgt && !_gps_hgt_intermittent && _gps_checks_passed) { - // In baro fallback mode and GPS has recovered so start using it - startGpsHgtFusion(); } } break; case VDIST_SENSOR_EV: - - // don't start using EV data unless data is arriving frequently, do not reset if pref mode was height + // don't start using EV data unless data is arriving frequently if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { - setControlEVHeight(); - - if (!_control_status_prev.flags.rng_hgt) { - resetHeight(); - } + startEvHgtFusion(); } break; @@ -908,6 +884,7 @@ void Ekf::controlHeightFusion() updateBaroHgtBias(); updateBaroHgtOffset(); + checkGroundEffectTimeout(); if (_control_status.flags.baro_hgt) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index c757fe092b..299dd3e3f1 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -915,10 +915,15 @@ private: void startBaroHgtFusion(); void startGpsHgtFusion(); + void startRngHgtFusion(); + void startRngAidHgtFusion(); + void startEvHgtFusion(); void updateBaroHgtOffset(); void updateBaroHgtBias(); + void checkGroundEffectTimeout(); + // return an estimation of the GPS altitude variance float getGpsHeightVariance(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 59b440a122..0848eb4d95 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1284,27 +1284,58 @@ void Ekf::startBaroHgtFusion() // We don't need to set a height sensor offset // since we track a separate _baro_hgt_offset _hgt_sensor_offset = 0.0f; - - // Turn off ground effect compensation if it times out - if (_control_status.flags.gnd_effect) { - if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { - - _control_status.flags.gnd_effect = false; - } - } } void Ekf::startGpsHgtFusion() { - setControlGPSHeight(); + if (!_control_status.flags.gps_hgt) { + setControlGPSHeight(); - // we have just switched to using gps height, calculate height sensor offset such that current - // measurement matches our current height estimate - if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) { + // calculate height sensor offset such that current + // measurement matches our current height estimate _hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2); } } +void Ekf::startRngHgtFusion() +{ + if (!_control_status.flags.rng_hgt) { + setControlRangeHeight(); + + // Range finder is the primary height source, the ground is now the datum used + // to compute the local vertical position + _hgt_sensor_offset = 0.f; + + if (!_control_status_prev.flags.ev_hgt) { + // EV and range finders are using the same height datum + resetHeight(); + } + } +} + +void Ekf::startRngAidHgtFusion() +{ + if (!_control_status.flags.rng_hgt) { + setControlRangeHeight(); + + // calculate height sensor offset such that current + // measurement matches our current height estimate + _hgt_sensor_offset = _terrain_vpos; + } +} + +void Ekf::startEvHgtFusion() +{ + if (!_control_status.flags.ev_hgt) { + setControlEVHeight(); + + if (!_control_status_prev.flags.rng_hgt) { + // EV and range finders are using the same height datum + resetHeight(); + } + } +} + void Ekf::updateBaroHgtOffset() { // calculate a filtered offset between the baro origin and local NED origin if we are not @@ -1351,6 +1382,17 @@ void Ekf::updateBaroHgtBias() } } +void Ekf::checkGroundEffectTimeout() +{ + // Turn off ground effect compensation if it times out + if (_control_status.flags.gnd_effect) { + if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { + + _control_status.flags.gnd_effect = false; + } + } +} + Vector3f Ekf::getVisionVelocityInEkfFrame() const { Vector3f vel; diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index a2821d94c8..a98924ef13 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -152,6 +152,7 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback) _sensor_simulator.runSeconds(1); EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion()); // WHEN: stopping GPS fusion _sensor_simulator.stopGps();