From 4ffdd2ed639e95fc5c55582451428d7386c1fb55 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 31 Oct 2022 17:02:30 -0400 Subject: [PATCH] add protections for EV and flow starting --- src/modules/ekf2/EKF/common.h | 2 +- src/modules/ekf2/EKF/ekf.cpp | 2 -- src/modules/ekf2/EKF/ekf.h | 3 +++ src/modules/ekf2/EKF/ev_control.cpp | 14 +++++++++++++- src/modules/ekf2/EKF/ev_pos_control.cpp | 2 +- src/modules/ekf2/EKF/ev_vel_control.cpp | 2 +- src/modules/ekf2/EKF/ev_yaw_control.cpp | 2 +- src/modules/ekf2/EKF/optical_flow_control.cpp | 18 +++++++++++++----- 8 files changed, 33 insertions(+), 12 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 4dbc69b9a2..5e565b8a70 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -67,7 +67,7 @@ using math::Utilities::updateYawInRotMat; // maximum sensor intervals in usec #define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec) -#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec) +#define EV_MAX_INTERVAL (uint64_t)1e5 ///< Maximum allowable time interval between external vision system measurements (uSec) #define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec) #define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 4cc769f253..4e3b7ed479 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -204,8 +204,6 @@ bool Ekf::initialiseFilter() _time_last_hgt_fuse = _imu_sample_delayed.time_us; _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; - _time_last_hagl_fuse = _imu_sample_delayed.time_us; - _time_last_flow_terrain_fuse = _imu_sample_delayed.time_us; // reset the output predictor state history to match the EKF initial values alignOutputFilter(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 220863a40f..58c3701e09 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -590,6 +590,9 @@ private: // Variables used to publish the WGS-84 location of the EKF local NED origin float _gps_alt_ref{NAN}; ///< WGS-84 height (m) + uint64_t _last_ev_fail_us{0}; ///< last system time in usec that the EV failed it's checks + uint64_t _last_ev_pass_us{0}; ///< last system time in usec that the EV passed it's checks + // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index 953adf21df..b098fcfab4 100644 --- a/src/modules/ekf2/EKF/ev_control.cpp +++ b/src/modules/ekf2/EKF/ev_control.cpp @@ -52,10 +52,22 @@ void Ekf::controlExternalVisionFusion() // determine if we should use the horizontal position observations bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum); + if (quality_sufficient) { + _last_ev_pass_us = _imu_sample_delayed.time_us; + + } else { + _last_ev_fail_us = _imu_sample_delayed.time_us; + } + + const bool ev_quality_passing = isTimedOut(_last_ev_fail_us, (uint64_t)1e6); + const bool ev_quality_failing = isTimedOut(_last_ev_pass_us, (uint64_t)1e6); + bool starting_conditions_passing = quality_sufficient + && ev_quality_passing + && !ev_quality_failing + && !ev_reset && ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL) && ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient - && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw); diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index d1ef291324..99378fb4a1 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -282,7 +282,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, bool starting_con aid_src.time_last_fuse = _imu_sample_delayed.time_us; - _nb_ev_pos_reset_available = 5; + _nb_ev_pos_reset_available = 3; _information_events.flags.starting_vision_pos_fusion = true; _control_status.flags.ev_pos = true; } diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index 979e951512..9c461317a0 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -214,7 +214,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_con aid_src.time_last_fuse = _imu_sample_delayed.time_us; - _nb_ev_vel_reset_available = 5; + _nb_ev_vel_reset_available = 3; _information_events.flags.starting_vision_vel_fusion = true; _control_status.flags.ev_vel = true; } diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index 79f675c6db..a0917da06d 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -169,7 +169,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, bool starting_con } if (_control_status.flags.ev_yaw) { - _nb_ev_yaw_reset_available = 5; + _nb_ev_yaw_reset_available = 3; } } } diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index f31f9eb936..5ad9d4f16b 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -50,6 +50,7 @@ void Ekf::controlOpticalFlowFusion() // Accumulate autopilot gyro data across the same time interval as the flow sensor const Vector3f delta_angle(_imu_sample_delayed.delta_ang - (getGyroBias() * _imu_sample_delayed.delta_ang_dt)); + if (_delta_time_of < 0.1f) { _imu_del_ang_of += delta_angle; _delta_time_of += _imu_sample_delayed.delta_ang_dt; @@ -142,10 +143,14 @@ void Ekf::controlOpticalFlowFusion() const bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5)) || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6))); + const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid(); + const bool starting_conditions_passing = (_flow_sample_delayed.quality >= _params.flow_qual_min) + && isTerrainEstimateValid(); + const bool inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required) - || !_control_status.flags.tilt_align; + || !_control_status.flags.tilt_align; // Handle cases where we are using optical flow but we should not use it anymore if (_control_status.flags.opt_flow) { @@ -160,7 +165,8 @@ void Ekf::controlOpticalFlowFusion() // optical flow fusion mode selection logic if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user && !_control_status.flags.opt_flow // we are not yet using flow data - && !inhibit_flow_use) { + && !inhibit_flow_use + && starting_conditions_passing) { // set the flag and reset the fusion timeout ECL_INFO("starting optical flow fusion"); @@ -195,7 +201,7 @@ void Ekf::controlOpticalFlowFusion() if (_imu_sample_delayed.time_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently // but use a relaxed time criteria to enable it to coast through bad range finder data - if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) { + if (isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) { fuseOptFlow(); _last_known_pos.xy() = _state.pos.xy(); } @@ -204,8 +210,10 @@ void Ekf::controlOpticalFlowFusion() } // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period - if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max) - && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) { + if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max) + && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow) + && isRecent(_time_last_hagl_fuse, (uint64_t)5e6) + ) { ECL_INFO("reset velocity to flow"); _information_events.flags.reset_vel_to_flow = true;