AP_NavEKF3: readyToUseBodyOdm uses delayed imu and wheel encoder timestamps

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -451,7 +451,7 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const
     bool visoDataGood = (imuSampleTime_ms - bodyOdmMeasTime_ms < 200) && (bodyOdmDataNew.velErr < 1.0f);
 
     // Check for fresh wheel encoder data
-    bool wencDataGood = (imuSampleTime_ms - wheelOdmMeasTime_ms < 200);
+    bool wencDataGood = (imuDataDelayed.time_ms - wheelOdmDataDelayed.time_ms < 200);
 
     // We require stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use odometry measurements
     // because they are in a body frame of reference

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -167,7 +167,6 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
     wheelOdmDataNew.delAng = delAng;
     wheelOdmDataNew.radius = radius;
     wheelOdmDataNew.delTime = delTime;
-    wheelOdmMeasTime_ms = timeStamp_ms;
 
     // because we are currently converting to an equivalent velocity measurement before fusing
     // the measurement time is moved back to the middle of the sampling period

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -402,7 +402,6 @@ void NavEKF3_core::InitialiseVariables()
     bodyOdmMeasTime_ms = 0;
     bodyVelFusionDelayed = false;
     bodyVelFusionActive = false;
-    wheelOdmMeasTime_ms = 0;
 
     // yaw sensor fusion
     yawMeasTime_ms = 0;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1260,7 +1260,6 @@ private:
     bool bodyVelFusionActive;           // true when body frame velocity fusion is active
 
     // wheel sensor fusion
-    uint32_t wheelOdmMeasTime_ms;       // time wheel odometry measurements were accepted for input to the data buffer (msec)
     obs_ring_buffer_t<wheel_odm_elements> storedWheelOdm;    // body velocity data buffer
     wheel_odm_elements wheelOdmDataDelayed;   // Body  frame odometry data at the fusion time horizon