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

EKF : Fix divergence when optical flow is not fused for a long time (#503) 

* terrain_estimator : guard against case where latest range sample is newer than IMU sample

* EKF : control : correct detection of no optical flow fusion over a time period
This commit is contained in:
Kabir Mohammed 2018-09-03 00:26:43 -04:00 committed by Paul Riseborough
parent b861594d0b
commit a7245229cb
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/control.cpp
View File

@ -420,7 +420,7 @@ void Ekf::controlOpticalFlowFusion()
_control_status.flags.opt_flow = false;
_time_last_of_fuse = 0;
} else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) {
} else if (_time_last_imu - _time_last_of_fuse > (uint64_t)_params.no_gps_timeout_max) {
_control_status.flags.opt_flow = false;
}

2
EKF/terrain_estimator.cpp
View File

@ -206,7 +206,7 @@ void Ekf::checkRangeDataContinuity()
/* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */
float alpha = 0.5f * _dt_update;
_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha *
(_time_last_imu - _time_last_range);
(_imu_sample_delayed.time_us - _range_sample_delayed.time_us);
_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f);