From 064ea0d46558d61452009f2e86a71984571f4bbe Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 26 Sep 2023 10:57:57 -0400 Subject: [PATCH] ekf2: optical flow control always use provided flow gyro (with bias applied) --- src/modules/ekf2/EKF/optflow_fusion.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 2db9f1c8d7..87b90e5206 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -219,13 +219,13 @@ bool Ekf::calcOptFlowBodyRateComp() // calculate the bias estimate using a combined LPF and spike filter _flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f; - - // apply gyro bias - _flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt); - - is_body_rate_comp_available = true; } + // apply gyro bias + _flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt); + + is_body_rate_comp_available = true; + } else { // Use the EKF gyro data if optical flow sensor gyro data is not available // for clarification of the sign see definition of flowSample and imuSample in common.h