From 5d34d7a24ef72b826c320a3259ee0ec68b1936df Mon Sep 17 00:00:00 2001 From: Eike Date: Wed, 31 Mar 2021 14:24:38 +0200 Subject: [PATCH] Height source vision: Fallback to rangefinder if available (#994) --- EKF/control.cpp | 17 ++++++++++++++--- EKF/ekf_helper.cpp | 14 ++++++++++++++ test/test_EKF_fusionLogic.cpp | 5 +++-- 3 files changed, 31 insertions(+), 5 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 1472d90e98..06761d4d1c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -787,7 +787,9 @@ void Ekf::controlHeightSensorTimeouts() const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us); // check if height has been inertial deadreckoning for too long - const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6); + // in vision hgt mode check for vision data + const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6) || + (_control_status.flags.ev_hgt && !isRecent(_time_last_ext_vision, 5 * EV_MAX_INTERVAL)); if (hgt_fusion_timeout || continuous_bad_accel_hgt) { @@ -876,6 +878,13 @@ void Ekf::controlHeightSensorTimeouts() failing_height_source = "ev"; new_height_source = "ev"; + // Fallback to rangefinder data if available + } else if (_range_sensor.isHealthy()) { + setControlRangeHeight(); + request_height_reset = true; + failing_height_source = "ev"; + new_height_source = "rng"; + } else if (!_baro_hgt_faulty) { startBaroHgtFusion(); @@ -1061,11 +1070,13 @@ void Ekf::controlHeightFusion() case VDIST_SENSOR_EV: - // don't start using EV data unless data is arriving frequently + // don't start using EV data unless data is arriving frequently, do not reset if pref mode was height if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { fuse_height = true; setControlEVHeight(); - resetHeight(); + if (!_control_status_prev.flags.rng_hgt) { + resetHeight(); + } } if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 8144c7a09b..c44e063686 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -262,6 +262,20 @@ void Ekf::resetHeight() // reset the vertical position if (_control_status.flags.rng_hgt) { + + // a fallback from any other height source to rangefinder happened + if(!_control_status_prev.flags.rng_hgt) { + + if (_control_status.flags.in_air && isTerrainEstimateValid()) { + _hgt_sensor_offset = _terrain_vpos; + } else if (_control_status.flags.in_air) { + _hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2); + } else { + _hgt_sensor_offset = _params.rng_gnd_clearance; + } + + } + // update the state and associated variance resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom()); diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index 4b22e53a3d..9430efbe55 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -379,6 +379,7 @@ TEST_F(EkfFusionLogicTest, doVisionHeightFusion) _sensor_simulator.runSeconds(12); // THEN: EKF should stop to intend to use vision height - // TODO: This is not happening - EXPECT_TRUE(_ekf_wrapper.isIntendingVisionHeightFusion()); // TODO: Needs to change + + EXPECT_FALSE(_ekf_wrapper.isIntendingVisionHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); }