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

AP_NavEKF2: Ensure corrected IMU data used by EKF 

Don't try to pop new IMU data from the buffer unless we have written data.
Correct IMU data as soon as it is popped from the buffer
This commit is contained in:
Paul Riseborough 2016-06-15 20:20:50 +10:00 committed by Andrew Tridgell
parent 1400dc9d02
commit fe06606193
2 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -299,17 +299,18 @@ void NavEKF2_core::readIMUData()
framesSincePredict = 0;
// set the flag to let the filter know it has new IMU data nad needs to run
runUpdates = true;
// extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU.pop_oldest_element();
float minDT = 0.1f*dtEkfAvg;
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
// correct the extracted IMU data for sensor errors
correctDeltaAngle(imuDataDelayed.delAng, imuDataDelayed.delAngDT);
correctDeltaVelocity(imuDataDelayed.delVel, imuDataDelayed.delVelDT);
} else {
// we don't have new IMU data in the buffer so don't run filter updates on this time step
runUpdates = false;
}
// extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU.pop_oldest_element();
float minDT = 0.1f*dtEkfAvg;
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
}
// read the delta velocity and corresponding time interval from the IMU

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -493,10 +493,6 @@ void NavEKF2_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT)
*/
void NavEKF2_core::UpdateStrapdownEquationsNED()
{
// apply corrections to the IMU data
correctDeltaAngle(imuDataDelayed.delAng, imuDataDelayed.delAngDT);
correctDeltaVelocity(imuDataDelayed.delVel, imuDataDelayed.delVelDT);
// apply correction for earth's rotation rate
// % * - and + operators have been overloaded
imuDataDelayed.delAng -= prevTnb * earthRateNED*imuDataDelayed.delAngDT;