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

Improve readibility

Browse Source
This commit is contained in:
kamilritz
2020-01-08 09:00:31 +01:00
committed by Mathieu Bresciani
parent 3daf25763e
commit 0b18143745
2 changed files with 13 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+10 -12
View File
@@ -367,24 +367,22 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// check if enough integration time and fail if integration time is less than 50%
// of min arrival interval because too much data is being lost
float delta_time = 1e-6f * (float)flow->dt; // in seconds
float delta_time_min = 5e-7f * (float)_min_obs_interval_us;
float delta_time_min = 0.5f * 1e-6f * (float)_min_obs_interval_us;
bool delta_time_good = delta_time >= delta_time_min;
if (!delta_time_good) {
bool flow_magnitude_good = true;
if (delta_time_good) {
// check magnitude is within sensor limits
// use this to prevent use of a saturated flow sensor
// when there are other aiding sources available
const float flow_rate_magnitude = flow->flowdata.norm() / delta_time;
flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate);
} else {
// protect against overflow caused by division with very small delta_time
delta_time = delta_time_min;
}
// 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;
bool flow_magnitude_good = true;
if (delta_time_good) {
flow_rate_magnitude = flow->flowdata.norm() / delta_time;
flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate);
}
const bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel;
const bool flow_quality_good = (flow->quality >= _params.flow_qual_min);