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

EKF: Fix false triggering of optical flow bad motion checks

Browse Source
This commit is contained in:
Paul Riseborough
2018-04-26 17:13:34 +10:00
parent f9d4934db6
commit 0a63052753
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+4 -3
View File
@@ -340,8 +340,8 @@ void Ekf::controlOpticalFlowFusion()
// Check if motion is un-suitable for use of optical flow // Check if motion is un-suitable for use of optical flow
if (!_control_status.flags.in_air) { if (!_control_status.flags.in_air) {
// Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated. // Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated.
bool motion_is_excessive = ((_accel_mag_filt > 10.8f) bool motion_is_excessive = ((_accel_mag_filt > 15.0f)
|| (_accel_mag_filt < 8.8f) || (_accel_mag_filt < 5.0f)
|| (_ang_rate_mag_filt > 0.5f) || (_ang_rate_mag_filt > 0.5f)
|| (_R_to_earth(2, 2) < 0.866f)); || (_R_to_earth(2, 2) < 0.866f));
@@ -353,7 +353,8 @@ void Ekf::controlOpticalFlowFusion()
} }
} else { } else {
if (_control_status.flags.gps && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6) && !_in_range_aid_mode) { bool good_gps_aiding = _control_status.flags.gps && ((_time_last_imu - _last_gps_fail_us) > (uint64_t)6e6);
if (good_gps_aiding && !_in_range_aid_mode) {
// Detect the special case where we are in flight, are using good quality GPS and speed and range has exceeded // Detect the special case where we are in flight, are using good quality GPS and speed and range has exceeded
// limits for use of range finder for height // limits for use of range finder for height
_time_bad_motion_us = _imu_sample_delayed.time_us; _time_bad_motion_us = _imu_sample_delayed.time_us;