From 0b1eba948a31c9f83fd48a32ddd94226df394adf Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 25 Jul 2024 15:02:57 +0200 Subject: [PATCH] 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. --- .../optical_flow/optical_flow_control.cpp | 23 +++++++++++++++---- src/modules/ekf2/EKF/common.h | 6 +++++ src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 2 ++ src/modules/ekf2/module.yaml | 10 ++++++++ 5 files changed, 37 insertions(+), 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 index 3c4c031f31..3299577a28 100644 --- 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(_params.flow_gyro_src)) { + default: - } 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); + /* 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; + } + + 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 index 6294310bbf..af03d2a8db 100644 --- 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(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 index ac98b9584f..bceb4d3dcc 100644 --- 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 index 326b0833a4..ef7280e88c 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -656,6 +656,8 @@ private: // optical flow fusion (ParamExtInt) _param_ekf2_of_ctrl, ///< optical flow fusion selection + (ParamExtInt) + _param_ekf2_of_gyr_src, (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/module.yaml b/src/modules/ekf2/module.yaml index e29c1b46cc..8ed6e700db 100644 --- 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