From 2a2e43b6202f40d7d7eb3ee3c2bb02cd08f5f25e Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 26 Apr 2023 14:32:53 +0200 Subject: [PATCH] ekf2: move flow fusion activation from AID_MASK to OF_CTRL --- src/modules/ekf2/EKF/common.h | 3 ++- src/modules/ekf2/EKF/estimator_interface.cpp | 2 +- src/modules/ekf2/EKF/optical_flow_control.cpp | 4 ++-- src/modules/ekf2/EKF2.cpp | 24 +++++++++++++++++++ src/modules/ekf2/EKF2.hpp | 2 ++ src/modules/ekf2/ekf2_params.c | 14 +++++++++-- .../test/sensor_simulator/ekf_wrapper.cpp | 4 ++-- 7 files changed, 45 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 72e04f0819..e29f307c18 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -158,7 +158,7 @@ enum class EvCtrl : uint8_t { enum SensorFusionMask : uint16_t { // Bit locations for fusion_mode DEPRECATED_USE_GPS = (1<<0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl) - USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data + DEPRECATED_USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data DEPRECATED_INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw @@ -423,6 +423,7 @@ struct parameters { float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2) #if defined(CONFIG_EKF2_OPTICAL_FLOW) + int32_t flow_ctrl{0}; float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval // optical flow fusion diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 10aefc3532..0955251586 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -569,7 +569,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } #if defined(CONFIG_EKF2_OPTICAL_FLOW) - if (_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) { + if (_params.flow_ctrl > 0) { max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms); } #endif // CONFIG_EKF2_OPTICAL_FLOW diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index d143cb4684..ae77e228cf 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -158,7 +158,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // 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 & SensorFusionMask::USE_OPT_FLOW) + if (!(_params.flow_ctrl == 1) || inhibit_flow_use) { stopFlowFusion(); @@ -167,7 +167,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) } // optical flow fusion mode selection logic - if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user + if ((_params.flow_ctrl == 1) // optical flow has been selected by the user && !_control_status.flags.opt_flow // we are not yet using flow data && !inhibit_flow_use) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b590d92b83..70de0c92a2 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -150,6 +150,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): #endif // CONFIG_EKF2_EXTERNAL_VISION _param_ekf2_grav_noise(_params->gravity_noise), #if defined(CONFIG_EKF2_OPTICAL_FLOW) + _param_ekf2_of_ctrl(_params->flow_ctrl), _param_ekf2_of_delay(_params->flow_delay_ms), _param_ekf2_of_n_min(_params->flow_noise), _param_ekf2_of_n_max(_params->flow_noise_qual_min), @@ -884,6 +885,29 @@ void EKF2::VerifyParams() } #endif // CONFIG_EKF2_DRAG_FUSION + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + + // IMU EKF2_AID_MASK -> EKF2_OF_CTRL (2023-04-26) + if (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_OPT_FLOW) { + // EKF2_OF_CTRL enable flow fusion + _param_ekf2_of_ctrl.set(1); + + // EKF2_AID_MASK clear deprecated bit + _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_OPT_FLOW)); + + _param_ekf2_of_ctrl.commit(); + _param_ekf2_aid_mask.commit(); + + mavlink_log_critical(&_mavlink_log_pub, "EKF2 optical flow use EKF2_OF_CTRL instead of EKF2_AID_MASK\n"); + /* EVENT + * @description EKF2_AID_MASK is set to {1:.0}. + */ + events::send(events::ID("ekf2_aid_mask_opt_flow"), events::Log::Warning, + "Use EKF2_OF_CTRL instead", _param_ekf2_aid_mask.get()); + } + +#endif // CONFIG_EKF2_OPTICAL_FLOW } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index ae1f0d0f06..74aade3dc2 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -640,6 +640,8 @@ private: #if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow fusion + (ParamExtInt) + _param_ekf2_of_ctrl, ///< optical flow fusion selection (ParamExtFloat) _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval (ParamExtFloat) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 4526ac0de1..dd7e955ad1 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -614,7 +614,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * * Set bits in the following positions to enable: * 0 : Deprecated, use EKF2_GPS_CTRL instead - * 1 : Set to true to use optical flow data if available + * 1 : Deprecated. use EKF2_OF_CTRL instead * 2 : Deprecated, use EKF2_IMU_CTRL instead * 3 : Deprecated, use EKF2_EV_CTRL instead * 4 : Deprecated, use EKF2_EV_CTRL instead @@ -627,7 +627,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * @min 0 * @max 511 * @bit 0 unused - * @bit 1 use optical flow + * @bit 1 unused * @bit 2 unused * @bit 3 unused * @bit 4 unused @@ -867,6 +867,16 @@ PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f); */ PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 1.0f); +/** + * Optical flow aiding + * + * Enable optical flow fusion. + * + * @group EKF2 + * @boolean + */ +PARAM_DEFINE_INT32(EKF2_OF_CTRL, 0); + /** * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum * diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 1be9c2c43a..adf31b1c1c 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -132,12 +132,12 @@ bool EkfWrapper::isIntendingGpsHeadingFusion() const void EkfWrapper::enableFlowFusion() { - _ekf_params->fusion_mode |= SensorFusionMask::USE_OPT_FLOW; + _ekf_params->flow_ctrl = 1; } void EkfWrapper::disableFlowFusion() { - _ekf_params->fusion_mode &= ~SensorFusionMask::USE_OPT_FLOW; + _ekf_params->flow_ctrl = 0; } bool EkfWrapper::isIntendingFlowFusion() const