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