Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Pos Vel reset after long GPS timeout

Browse Source
This commit is contained in:
Paul Riseborough 2014-01-26 08:49:25 +11:00 committed by Andrew Tridgell
parent 7ac0172db1
commit deb64c4cbd
2 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -624,6 +624,11 @@ void NavEKF::SelectVelPosFusion()
fusePosData = true; fusePosData = true;
// reset the counter used to skip updates so that we always fuse data on the frame data arrives // reset the counter used to skip updates so that we always fuse data on the frame data arrives
skipCounter = 0; skipCounter = 0;
// If a long time sinc last GPS update, then reset position and velocity
if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed)) {
ResetPosition();
ResetVelocity();
}
} }
// Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives // measurement until the next one arrives
@ -2455,6 +2460,7 @@ void NavEKF::readGpsData()
if ((_ahrs->get_gps()->last_message_time_ms() != lastFixTime_ms) && if ((_ahrs->get_gps()->last_message_time_ms() != lastFixTime_ms) &&
(_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D)) (_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D))
{ {
secondLastFixTime_ms = lastFixTime_ms;
lastFixTime_ms = _ahrs->get_gps()->last_message_time_ms(); lastFixTime_ms = _ahrs->get_gps()->last_message_time_ms();
newDataGps = true; newDataGps = true;
RecallStates(statesAtVelTime, (IMUmsec - constrain_int16(_msecVelDelay, 0, 500))); RecallStates(statesAtVelTime, (IMUmsec - constrain_int16(_msecVelDelay, 0, 500)));
@ -2611,6 +2617,7 @@ void NavEKF::ZeroVariables()
posTimeout = false; posTimeout = false;
hgtTimeout = false; hgtTimeout = false;
lastFixTime_ms = 0; lastFixTime_ms = 0;
secondLastFixTime_ms = 0;
lastMagUpdate = 0; lastMagUpdate = 0;
lastAirspeedUpdate = 0; lastAirspeedUpdate = 0;
velFailTime = 0; velFailTime = 0;

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -355,6 +355,7 @@ private:
uint32_t hgtFailTime; // time stamp when height measurement last failed covaraiance consistency check (msec) uint32_t hgtFailTime; // time stamp when height measurement last failed covaraiance consistency check (msec)
uint8_t storeIndex; // State vector storage index uint8_t storeIndex; // State vector storage index
uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived
uint32_t secondLastFixTime_ms; // time of second last GPS fix used to determine how long since last update
Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator
Vector3f lastAccel; // acceleration from previous IMU sample used for trapezoidal integrator Vector3f lastAccel; // acceleration from previous IMU sample used for trapezoidal integrator
Matrix22 nextP; // Predicted covariance matrix before addition of process noise to diagonals Matrix22 nextP; // Predicted covariance matrix before addition of process noise to diagonals