Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Fix bug that limits copter sensor delay compensation to 125

Browse Source 
msec
This commit is contained in:
Paul Riseborough 2014-03-21 20:56:32 +11:00 committed by Andrew Tridgell
parent b2c0979947
commit f7007569d1
2 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2498,10 +2498,18 @@ void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last)
// store states in a history array along with time stamp
void NavEKF::StoreStates()
{
if (storeIndex > 49) storeIndex = 0;
for (uint8_t i=0; i<=30; i++) storedStates[i][storeIndex] = states[i];
statetimeStamp[storeIndex] = hal.scheduler->millis();
storeIndex = storeIndex + 1;
// Don't need to store states more often than every 10 msec
if (hal.scheduler->millis() - lastStateStoreTime_ms >= 10) {
lastStateStoreTime_ms = hal.scheduler->millis();
if (storeIndex > 49) {
storeIndex = 0;
}
for (uint8_t i=0; i<=30; i++) {
storedStates[i][storeIndex] = states[i];
}
statetimeStamp[storeIndex] = lastStateStoreTime_ms;
storeIndex = storeIndex + 1;
}
}
// reset the stored state history and store the current state
@ -3080,6 +3088,7 @@ void NavEKF::ZeroVariables()
velTimeout = false;
posTimeout = false;
hgtTimeout = false;
lastStateStoreTime_ms = 0;
lastFixTime_ms = 0;
secondLastFixTime_ms = 0;
lastMagUpdate = 0;

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -405,6 +405,7 @@ private:
uint32_t posFailTime; // time stamp when GPS position 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
uint32_t lastStateStoreTime_ms; // time of last state vector storage
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