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

EKF: Relax minimum required range finder measurement rate

Browse Source
This commit is contained in:
Paul Riseborough 2018-05-05 16:33:01 +10:00
parent 02963a85c4
commit 868bc01087
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
EKF/terrain_estimator.cpp
View File

@ -207,16 +207,17 @@ void Ekf::get_hagl_innov_var(float *hagl_innov_var)
// check that the range finder data is continuous
void Ekf::checkRangeDataContinuity()
{
// update range data continuous flag (2Hz ie 500 ms)
// update range data continuous flag (1Hz ie 1000 ms)
/* Timing in micro seconds */
/* Apply a 1.0 sec low pass filter to the time delta from the last range finder updates */
_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - _dt_update) + _dt_update *
/* 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);
_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 1e6f);
_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 2e6f);
if (_dt_last_range_update_filt_us < 5e5f) {
if (_dt_last_range_update_filt_us < 1e6f) {
_range_data_continuous = true;
} else {