fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 15:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Don't reject saturated flow data when it is the only aiding source

Browse Source
This commit is contained in:
Paul Riseborough
2018-05-09 15:32:57 +10:00
parent bf902e5eca
commit 6cadc92285
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+6 -2
View File
@@ -364,20 +364,24 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// check magnitude is within sensor limits // check magnitude is within sensor limits
// use this to prevent use of a saturated flow sensor when there are other aiding sources available
float flow_rate_magnitude; float flow_rate_magnitude;
bool flow_magnitude_good = true; bool flow_magnitude_good = true;
if (delta_time_good) { if (delta_time_good) {
flow_rate_magnitude = flow->flowdata.norm() / delta_time; flow_rate_magnitude = flow->flowdata.norm() / delta_time;
flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max); flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max);
} }
bool relying_on_flow = _control_status.flags.opt_flow
&& !_control_status.flags.gps
&& !_control_status.flags.ev_pos;
// check quality metric // check quality metric
bool flow_quality_good = (flow->quality >= _params.flow_qual_min); bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
// Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling // Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling
// If flow quality fails checks on ground, assume zero flow rate after body rate compensation // 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) || !_control_status.flags.in_air) { if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !_control_status.flags.in_air) {
flowSample optflow_sample_new; flowSample optflow_sample_new;
// calculate the system time-stamp for the mid point of the integration period // 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; optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;