From 2fb7b35a8b9ac246fcf002100de50d22fa575b2f Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 8 Feb 2022 16:28:42 +0100 Subject: [PATCH] ekf2_terr: refactor terrain estimator - flow aiding --- src/modules/ekf2/EKF/control.cpp | 13 +--- src/modules/ekf2/EKF/ekf.h | 7 ++- src/modules/ekf2/EKF/terrain_estimator.cpp | 71 +++++++++++++++++++--- 3 files changed, 66 insertions(+), 25 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 444832fa84..4f597b50d3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -134,19 +134,9 @@ void Ekf::controlFusionModes() // We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon. // This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow, // in this case we need to empty the buffer - if (!_flow_data_ready || !_control_status.flags.opt_flow) { + if (!_flow_data_ready || (!_control_status.flags.opt_flow && !_hagl_sensor_status.flags.flow)) { _flow_data_ready = _flow_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed); } - - // check if we should fuse flow data for terrain estimation - if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) { - // TODO: WARNING, _flow_data_ready can be modified in controlOpticalFlowFusion - // due to some checks failing - // only fuse flow for terrain if range data hasn't been fused for 5 seconds - _flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6); - // only fuse flow for terrain if the main filter is not fusing flow and we are using gps - _flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps); - } } if (_ext_vision_buffer) { @@ -420,7 +410,6 @@ void Ekf::controlOpticalFlowFusion() } else { // don't use this flow data and wait for the next data to arrive _flow_data_ready = false; - _flow_for_terrain_data_ready = false; // TODO: find a better place } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 6ceffd79be..27e26e9c17 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -680,8 +680,6 @@ private: // initialise the terrain vertical position estimator void initHagl(); - bool shouldUseOpticalFlowForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); } - void runTerrainEstimator(); void predictHagl(); @@ -695,8 +693,11 @@ private: float getRngVar(); // update the terrain vertical position estimate using an optical flow measurement - void fuseFlowForTerrain(); + void controlHaglFlowFusion(); + void startHaglFlowFusion(); void resetHaglFlow(); + void stopHaglFlowFusion(); + void fuseFlowForTerrain(); void controlHaglFakeFusion(); void resetHaglFake(); diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index dcb6028138..3a1b4d072f 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -57,15 +57,8 @@ void Ekf::runTerrainEstimator() predictHagl(); - // Fuse range finder data if available controlHaglRngFusion(); - - if (shouldUseOpticalFlowForHagl() - && _flow_for_terrain_data_ready) { - fuseFlowForTerrain(); - _flow_for_terrain_data_ready = false; - } - + controlHaglFlowFusion(); controlHaglFakeFusion(); // constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) @@ -239,8 +232,68 @@ void Ekf::fuseHaglRng() } } +void Ekf::controlHaglFlowFusion() +{ + if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) { + stopHaglFlowFusion(); + return; + } + + 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; */ + const bool starting_conditions_passing = continuing_conditions_passing; + + if (_hagl_sensor_status.flags.flow) { + if (continuing_conditions_passing) { + + // TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon + fuseFlowForTerrain(); + _flow_data_ready = false; + + // TODO: do something when failing continuously the innovation check + /* const bool is_fusion_failing = isTimedOut(_time_last_flow_terrain_fuse, _params.reset_timeout_max); */ + + /* if (is_fusion_failing) { */ + /* resetHaglFlow(); */ + /* } */ + + } else { + stopHaglFlowFusion(); + } + + } else { + if (starting_conditions_passing) { + startHaglFlowFusion(); + } + } + + } else if (_hagl_sensor_status.flags.flow + && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) { + // No data anymore. Stop until it comes back. + stopHaglFlowFusion(); + } +} + +void Ekf::startHaglFlowFusion() +{ + _hagl_sensor_status.flags.flow = true; + // TODO: do a reset instead of trying to fuse the data? + fuseFlowForTerrain(); + _flow_data_ready = false; +} + +void Ekf::stopHaglFlowFusion() +{ + _hagl_sensor_status.flags.flow = false; +} + void Ekf::resetHaglFlow() { + // TODO: use the flow data _terrain_vpos = fmaxf(0.0f, _state.pos(2)); _terrain_var = 100.0f; _terrain_vpos_reset_counter++; @@ -374,6 +427,4 @@ void Ekf::updateTerrainValidity() const bool recent_flow_for_terrain_fusion = isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6); _hagl_valid = (recent_range_fusion || recent_flow_for_terrain_fusion); - - _hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion; }