fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 10:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-flow: add param to force using internal gyro

Browse Source 
In some cases the vibration environment of the optical flow sensor is
worse than near the autopilot.
This commit is contained in:
bresch
2024-07-25 19:54:19 -04:00
committed by Daniel Agar
parent a07c986dbc
commit 0b1eba948a
5 changed files with 37 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+18 -5
View File
@@ -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:
} 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;
src/modules/ekf2/EKF/common.h
+6
View File
@@ -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