Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

INav: degrade pos error slowly on loss of GPS 

When GPS message is late by 100ms or we are glitching, degrade the GPS
vs inertial nav position error to 10% over 2 seconds instead of
immediately setting it to zero.  This avoids jumpy position estimates
when the GPS misses an update
This commit is contained in:
Randy Mackay 2013-11-18 23:19:57 +09:00
parent 24dc4391bb
commit a147eeb1e3
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -133,10 +133,10 @@ void AP_InertialNav::check_gps()
// record gps time and system time of this update
_gps_last_time = _gps->last_fix_time;
}else{
// clear position error if GPS updates stop arriving
// if GPS updates stop arriving degrade position error to 10% over 2 seconds (assumes 100hz update rate)
if (now - _gps_last_update > AP_INTERTIALNAV_GPS_TIMEOUT_MS) {
_position_error.x = 0;
_position_error.y = 0;
_position_error.x *= 0.9886;
_position_error.y *= 0.9886;
}
}
}
@ -164,9 +164,9 @@ void AP_InertialNav::correct_with_gps(uint32_t now, int32_t lon, int32_t lat)
// sanity check the gps position. Relies on the main code calling GPS_Glitch::check_position() immediatley after a GPS update
if (_glitch_detector.glitching()) {
// failed sanity check so set position_error to zero
_position_error.x = 0;
_position_error.y = 0;
// failed sanity check so degrate position_error to 10% over 2 seconds (assumes 5hz update rate)
_position_error.x *= 0.7934;
_position_error.y *= 0.7934;
}else{
// if our internal glitching flag (from previous iteration) is true we have just recovered from a glitch
// reset the inertial nav position and velocity to gps values