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

EKF: Harden GPS offset filter value for HIL

This commit is contained in:
Lorenz Meier 2015-06-08 11:23:18 +02:00
parent f02ffa5a90
commit e1ecac078d
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -1321,7 +1321,11 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
// update LPF
_gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
float filter_step += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
if (isfinite(filter_step)) {
_gps_alt_filt += filter_step;
}
}
//check if we had a GPS outage for a long time