From f1658aeaf89d077abd4757834afe7c3e533f2268 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 22 Feb 2022 16:03:20 +0100 Subject: [PATCH] hagl: fix flow for terrain starting unit tests are passing --- src/modules/ekf2/EKF/control.cpp | 2 ++ src/modules/ekf2/EKF/terrain_estimator.cpp | 6 +----- src/modules/ekf2/test/test_EKF_terrain_estimator.cpp | 2 +- 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 97ec445b87..d07c355773 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -437,6 +437,8 @@ void Ekf::controlOpticalFlowFusion() _inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required) || !_control_status.flags.tilt_align; + controlHaglFlowFusion(); + // Handle cases where we are using optical flow but we should not use it anymore if (_control_status.flags.opt_flow) { if (!(_params.fusion_mode & MASK_USE_OF) diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 882d8906e5..1211e57d22 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -57,7 +57,6 @@ void Ekf::controlHaglFusion() } controlHaglRngFusion(); - controlHaglFlowFusion(); controlHaglFakeFusion(); // constrain terrain position to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) @@ -188,10 +187,7 @@ void Ekf::controlHaglFlowFusion() if (_flow_data_ready) { const bool continuing_conditions_passing = _control_status.flags.in_air - && !_control_status.flags.opt_flow - && _control_status.flags.gps - && isTimedOut(_time_last_hagl_fuse, 5e6f); // TODO: check for range_finder hagl aiding instead? - /* && !_hagl_sensor_status.flags.range_finder; */ + && _control_status.flags.gps; const bool starting_conditions_passing = continuing_conditions_passing; if (_hagl_sensor_status.flags.flow) { diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp index 1c993e78d2..eacddd1c79 100644 --- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp @@ -112,7 +112,7 @@ TEST_F(EkfTerrainTest, setFlowAndRangeTerrainFusion) // THEN: By default, both rng and flow aiding are active EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainRngFusion()); - EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainFlowFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion()); const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); EXPECT_NEAR(estimated_distance_to_ground, simulated_distance_to_ground, 1e-3f);