fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 18:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix half frame offset in flow gyro compensation.

Browse Source 
The gyro data accumulation needs to be across the same integration period as the flow sensor. The previous code didn't sample the accumulation until the midpoint of the flow data had fallen behind the fusion time horizon.

This PR changes the optical flow time stamp definition so that flow data is retrieved when the leading edge of the flow accumulation period falls behind the fusion time horizon. This enables the accumulated gyro data to be sampled at the correct time. Fusion is then delayed until the mid sample time has fallen behind the fusion time horizon.
This commit is contained in:
Paul Riseborough
2018-06-13 16:51:42 +10:00
parent 48561b0c8a
commit c6ed2ccfcd
4 changed files with 31 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+2 -2
View File
@@ -396,8 +396,8 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// If flow quality fails checks on ground, assume zero flow rate after body rate compensation
if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !_control_status.flags.in_air) {
flowSample optflow_sample_new;
// calculate the system time-stamp for the mid point of the integration period
optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;
// calculate the system time-stamp for the leading edge of the flow data integration period
optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000;
// copy the quality metric returned by the PX4Flow sensor
optflow_sample_new.quality = flow->quality;