mirror of https://github.com/ArduPilot/ardupilot
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:
parent
1400dc9d02
commit
fe06606193
|
@ -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;
|
||||
} 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);
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
// read the delta velocity and corresponding time interval from the IMU
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue