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.

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))) {
+		switch (static_cast<FlowGyroSource>(_params.flow_gyro_src)) {
-			_flow_sample_delayed.gyro_rate = _ref_body_rate;
+		default:
 
-		} else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(2))) {
+		/* FALLTHROUGH */
-			// Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro
+		case FlowGyroSource::Auto:
-			_flow_sample_delayed.gyro_rate(2) = _ref_body_rate(2);
+			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;
+			}
+
+			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;

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

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),

src/modules/ekf2/EKF2.hpp

@@ -656,6 +656,8 @@ private:
 		// 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>)

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