dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: optical flow control always use provided flow gyro (with bias applied)

This commit is contained in:
Daniel Agar 2023-09-26 10:57:57 -04:00
parent 291af17b64
commit 064ea0d465
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/ekf2/EKF/optflow_fusion.cpp
View File

@ -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