ekf2-flow: add param to force using internal gyro

In some cases the vibration environment of the optical flow sensor is worse than near the autopilot.
PX4 · Jul 25, 2024 · 0b1eba9 · 0b1eba9
1 parent a07c986
commit 0b1eba9
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 5 deletions.
diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
@@ -56,12 +56,25 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		_ref_body_rate = -(imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias());
 
 		// ensure valid flow sample gyro rate before proceeding
-		if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_rate(1))) {
-			_flow_sample_delayed.gyro_rate = _ref_body_rate;
+		switch (static_cast<FlowGyroSource>(_params.flow_gyro_src)) {
+		default:
+
+		/* FALLTHROUGH */
+		case FlowGyroSource::Auto:
+			if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_rate(1))) {
+				_flow_sample_delayed.gyro_rate = _ref_body_rate;
+			}
 
-		} else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(2))) {
-			// Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro
-			_flow_sample_delayed.gyro_rate(2) = _ref_body_rate(2);
+			if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(2))) {
+				// Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro
+				_flow_sample_delayed.gyro_rate(2) = _ref_body_rate(2);
+			}
+
+			break;
+
+		case FlowGyroSource::Internal:
+			_flow_sample_delayed.gyro_rate = _ref_body_rate;
+			break;
 		}
 
 		const flowSample &flow_sample = _flow_sample_delayed;

diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
@@ -167,6 +167,11 @@ enum class MagCheckMask : uint8_t {
 	FORCE_WMM   = (1 << 2)
 };
 
+enum class FlowGyroSource : uint8_t {
+	Auto     = 0,
+	Internal = 1
+};
+
 struct imuSample {
 	uint64_t    time_us{};                ///< timestamp of the measurement (uSec)
 	Vector3f    delta_ang{};              ///< delta angle in body frame (integrated gyro measurements) (rad)
@@ -442,6 +447,7 @@ struct parameters {
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	int32_t flow_ctrl {0};
+	int32_t flow_gyro_src {static_cast<int32_t>(FlowGyroSource::Auto)};
 	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/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
@@ -181,6 +181,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	_param_ekf2_of_ctrl(_params->flow_ctrl),
+	_param_ekf2_of_gyr_src(_params->flow_gyro_src),
 	_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),

diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
@@ -656,6 +656,8 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 		// optical flow fusion
 		(ParamExtInt<px4::params::EKF2_OF_CTRL>)
 		_param_ekf2_of_ctrl, ///< optical flow fusion selection
+		(ParamExtInt<px4::params::EKF2_OF_GYR_SRC>)
+		_param_ekf2_of_gyr_src,
 		(ParamExtFloat<px4::params::EKF2_OF_DELAY>)
 		_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<px4::params::EKF2_OF_N_MIN>)

diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml
@@ -671,6 +671,16 @@ parameters:
         long: Enable optical flow fusion.
       type: boolean
       default: 1
+    EKF2_OF_GYR_SRC:
+      description:
+        short: Optical flow angular rate compensation source
+        long: 'Auto: use gyro from optical flow message if available, internal gyro otherwise.
+               Internal: always use internal gyro'
+      type: enum
+      values:
+        0: Auto
+        1: Internal
+      default: 0
     EKF2_OF_N_MIN:
       description:
         short: Optical flow minimum noise