aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix bug causing incorrect fusion timing

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-21 10:10:11 +11:00 committed by Andrew Tridgell
parent 6b687ea7bc
commit 4640673cb1
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -386,12 +386,17 @@ void NavEKF2_core::readIMUData()
// store imu in the FIFO
void NavEKF2_core::StoreIMU()
{
fifoIndexDelayed = fifoIndexNow;
// increment the index and write new data
fifoIndexNow = fifoIndexNow + 1;
if (fifoIndexNow >= IMU_BUFFER_LENGTH) {
fifoIndexNow = 0;
}
storedIMU[fifoIndexNow] = imuDataNew;
// set the index required to access the oldest data
fifoIndexDelayed = fifoIndexNow + 1;
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
fifoIndexDelayed = 0;
}
}
// reset the stored imu history and store the current value