AP_NavEKF : Clean up time stamps

Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -369,7 +369,7 @@ bool NavEKF::healthy(void) const
     if (state.velocity.is_nan()) {
         return false;
     }
-    if (filterDiverged || (hal.scheduler->millis() - lastDivergeTime_ms < 10000)) {
+    if (filterDiverged || (imuSampleTime_ms - lastDivergeTime_ms < 10000)) {
         return false;
     }
     // If measurements have failed innovation consistency checks for long enough to time-out
@@ -680,12 +680,12 @@ void NavEKF::SelectVelPosFusion()
             // If a long time since last GPS update, then reset position and velocity and reset stored state history
 
             uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS;
-            if (hal.scheduler->millis() - secondLastFixTime_ms > gpsRetryTimeout) {
+            if (imuSampleTime_ms - secondLastFixTime_ms > gpsRetryTimeout) {
                 ResetPosition();
                 ResetVelocity();
                 StoreStatesReset();
             }
-        } else if (hal.scheduler->millis() - lastFixTime_ms > (uint32_t)(_msecGpsAvg + 40)) {
+        } else if (imuSampleTime_ms - lastFixTime_ms > (uint32_t)(_msecGpsAvg + 40)) {
             // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same
             // measurement until the next one arrives to provide a smoother output
             fuseVelData = false;
@@ -699,7 +699,7 @@ void NavEKF::SelectVelPosFusion()
             newDataHgt = false;
             // enable fusion
             fuseHgtData = true;
-        } else if (hal.scheduler->millis() - lastHgtTime_ms > (uint32_t)(_msecHgtAvg + 40)) {
+        } else if (imuSampleTime_ms - lastHgtTime_ms > (uint32_t)(_msecHgtAvg + 40)) {
             // timeout fusion of height data if stale. Needed because we repeatedly fuse the same
             // measurement until the next one arrives to provide a smoother output
             fuseHgtData = false;
@@ -746,9 +746,9 @@ void NavEKF::SelectMagFusion()
         // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
         // If we have a vehicle that can fly without a compass (a vehicle that doesn't have significant sideslip) then the compass is permanently failed and will not be used until the filter is reset
         if (magHealth) {
-            lastHealthyMagTime_ms = hal.scheduler->millis();
+            lastHealthyMagTime_ms = imuSampleTime_ms;
         } else {
-            if ((hal.scheduler->millis() - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) {
+            if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) {
                 magTimeout = true;
                 if (assume_zero_sideslip()) {
                     magFailed = true;
@@ -762,7 +762,7 @@ void NavEKF::SelectMagFusion()
         bool dataReady = statesInitialised && use_compass() && newDataMag;
         if (dataReady)
         {
-            MAGmsecPrev = IMUmsec;
+            MAGmsecPrev = imuSampleTime_ms;
             fuseMagData = true;
         }
         else
@@ -785,7 +785,7 @@ void NavEKF::SelectTasFusion()
     tasDataWaiting = (statesInitialised && !inhibitWindStates && (tasDataWaiting || newDataTas));
 
     // if we have waited too long, set a timeout flag which will force fusion to occur
-    bool timeout = ((IMUmsec - TASmsecPrev) >= TASmsecMax);
+    bool timeout = ((imuSampleTime_ms - TASmsecPrev) >= TASmsecMax);
 
     // we don't fuse airspeed measurements if magnetometer fusion has been performed in the same frame, unless timed out or the fuseMeNow option is selected
     // this helps to spreasthe load associated with fusion of different measurements across multiple frames
@@ -793,7 +793,7 @@ void NavEKF::SelectTasFusion()
     if (tasDataWaiting && (!magFusePerformed || timeout || fuseMeNow))
     {
         FuseAirspeed();
-        TASmsecPrev = IMUmsec;
+        TASmsecPrev = imuSampleTime_ms;
         tasDataWaiting = false;
     }
 }
@@ -808,9 +808,9 @@ void NavEKF::SelectBetaFusion()
     // we are a fly forward vehicle type AND NOT using a full range of sensors with healthy position
     // AND NOT on the ground AND enough time has lapsed since our last fusion
     // AND (we have not fused magnetometer data on this time step OR the immediate fusion flag is set)
-    if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !inhibitWindStates  && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) {
+    if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !inhibitWindStates  && ((imuSampleTime_ms - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) {
         FuseSideslip();
-        BETAmsecPrev = IMUmsec;
+        BETAmsecPrev = imuSampleTime_ms;
     }
 }
 
@@ -1679,15 +1679,15 @@ void NavEKF::FuseVelPosNED()
             // calculate max valid position innovation squared based on a maximum horizontal inertial nav accel error and GPS noise parameter
             // max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring
             float accelScale =  (1.0f + 0.1f * accNavMagHoriz);
-            float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(hal.scheduler->millis() - posFailTime)));
+            float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime)));
             posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2;
             posHealth = ((posTestRatio < 1.0f) || badIMUdata);
             // declare a timeout condition if we have been too long without data
-            posTimeout = ((hal.scheduler->millis() - posFailTime) > gpsRetryTime);
+            posTimeout = ((imuSampleTime_ms - posFailTime) > gpsRetryTime);
             // use position data if healthy, timed out, or in static mode
             if (posHealth || posTimeout || staticMode) {
                 posHealth = true;
-                posFailTime = hal.scheduler->millis();
+                posFailTime = imuSampleTime_ms;
                 // if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps
                 if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) {
                     gpsPosGlitchOffsetNE.x += posInnov[0];
@@ -1748,11 +1748,11 @@ void NavEKF::FuseVelPosNED()
             // fail if the ratio is greater than 1
             velHealth = ((velTestRatio < 1.0f)  || badIMUdata);
             // declare a timeout if we have not fused velocity data for too long
-            velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime;
+            velTimeout = (imuSampleTime_ms - velFailTime) > gpsRetryTime;
             // if data is healthy  or in static mode we fuse it
             if (velHealth || staticMode) {
                 velHealth = true;
-                velFailTime = hal.scheduler->millis();
+                velFailTime = imuSampleTime_ms;
             } else if (velTimeout && !posHealth) {
                 // if data is not healthy and timed out and position is unhealthy we reset the velocity, but do not fuse data on this time step
                 ResetVelocity();
@@ -1777,11 +1777,11 @@ void NavEKF::FuseVelPosNED()
             hgtTestRatio = sq(hgtInnov) / (sq(_hgtInnovGate) * varInnovVelPos[5]);
             // fail if the ratio is > 1, but don't fail if bad IMU data
             hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
-            hgtTimeout = (hal.scheduler->millis() - hgtFailTime) > hgtRetryTime;
+            hgtTimeout = (imuSampleTime_ms - hgtFailTime) > hgtRetryTime;
             // Fuse height data if healthy or timed out or in static mode
             if (hgtHealth || hgtTimeout || staticMode) {
                 hgtHealth = true;
-                hgtFailTime = hal.scheduler->millis();
+                hgtFailTime = imuSampleTime_ms;
                 // if timed out, reset the height, but do not fuse data on this time step
                 if (hgtTimeout) {
                     ResetHeight();
@@ -2681,8 +2681,8 @@ void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last)
 void NavEKF::StoreStates()
 {
     // 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 (imuSampleTime_ms - lastStateStoreTime_ms >= 10) {
+        lastStateStoreTime_ms = imuSampleTime_ms;
         if (storeIndex > 49) {
             storeIndex = 0;
         }
@@ -2701,7 +2701,7 @@ void NavEKF::StoreStatesReset()
     // store current state vector in first column
     storeIndex = 0;
     storedStates[storeIndex] = state;
-    statetimeStamp[storeIndex] = hal.scheduler->millis();
+    statetimeStamp[storeIndex] = imuSampleTime_ms;
     storeIndex = storeIndex + 1;
 }
 
@@ -2925,7 +2925,7 @@ void NavEKF::ForceSymmetry()
                 // set the filter status as diverged and re-initialise the filter
                 filterDiverged = true;
                 faultStatus.diverged = true;
-                lastDivergeTime_ms = hal.scheduler->millis();
+                lastDivergeTime_ms = imuSampleTime_ms;
                 InitialiseFilterDynamic();
                 return;
             }
@@ -2996,8 +2996,8 @@ void NavEKF::readIMUData()
     Vector3f accel1;    // acceleration vector in XYZ body axes measured by IMU1 (m/s^2)
     Vector3f accel2;    // acceleration vector in XYZ body axes measured by IMU2 (m/s^2)
 
-    // get the time the IMU data was read
-    IMUmsec = hal.scheduler->millis();
+    // the imu sample time is sued as a common time reference throughout the filter
+    imuSampleTime_ms = hal.scheduler->millis();
 
     // limit IMU delta time to prevent numerical problems elsewhere
     dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(), 0.001f, 1.0f);
@@ -3051,8 +3051,8 @@ void NavEKF::readGpsData()
 
         // get state vectors that were stored at the time that is closest to when the the GPS measurement
         // time after accounting for measurement delays
-        RecallStates(statesAtVelTime, (IMUmsec - constrain_int16(_msecVelDelay, 0, 500)));
-        RecallStates(statesAtPosTime, (IMUmsec - constrain_int16(_msecPosDelay, 0, 500)));
+        RecallStates(statesAtVelTime, (imuSampleTime_ms - constrain_int16(_msecVelDelay, 0, 500)));
+        RecallStates(statesAtPosTime, (imuSampleTime_ms - constrain_int16(_msecPosDelay, 0, 500)));
 
         // read the NED velocity from the GPS
         velNED = _ahrs->get_gps().velocity();
@@ -3082,14 +3082,14 @@ void NavEKF::readHgtData()
         lastHgtMeasTime = _baro.get_last_update();
 
         // time stamp used to check for timeout
-        lastHgtTime_ms = hal.scheduler->millis();
+        lastHgtTime_ms = imuSampleTime_ms;
 
         // get measurement and set flag to let other functions know new data has arrived
         hgtMea = _baro.get_altitude();
         newDataHgt = true;
 
         // get states that wer stored at the time closest to the measurement time, taking measurement delay into account
-        RecallStates(statesAtHgtTime, (IMUmsec - _msecHgtDelay));
+        RecallStates(statesAtHgtTime, (imuSampleTime_ms - _msecHgtDelay));
     } else {
         newDataHgt = false;
     }
@@ -3109,7 +3109,7 @@ void NavEKF::readMagData()
         magData = _ahrs->get_compass()->get_field() * 0.001f + magBias;
 
         // get states stored at time closest to measurement time after allowance for measurement delay
-        RecallStates(statesAtMagMeasTime, (IMUmsec - _msecMagDelay));
+        RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - _msecMagDelay));
 
         // let other processes know that new compass data has arrived
         newDataMag = true;
@@ -3131,7 +3131,7 @@ void NavEKF::readAirSpdData()
         VtasMeas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
         lastAirspeedUpdate = aspeed->last_update_ms();
         newDataTas = true;
-        RecallStates(statesAtVtasMeasTime, (IMUmsec - _msecTasDelay));
+        RecallStates(statesAtVtasMeasTime, (imuSampleTime_ms - _msecTasDelay));
     } else {
         newDataTas = false;
     }
@@ -3311,29 +3311,30 @@ void  NavEKF::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f
 // zero stored variables - this needs to be called before a full filter initialisation
 void NavEKF::ZeroVariables()
 {
+    // initialise time stamps
+    imuSampleTime_ms = hal.scheduler->millis();
+    lastHealthyMagTime_ms = imuSampleTime_ms;
+    lastDivergeTime_ms = imuSampleTime_ms;
+    TASmsecPrev = imuSampleTime_ms;
+    BETAmsecPrev = imuSampleTime_ms;
+    lastMagUpdate = imuSampleTime_ms;
+    lastHgtMeasTime = imuSampleTime_ms;
+    lastHgtTime_ms = imuSampleTime_ms;
+    velFailTime = imuSampleTime_ms;
+    posFailTime = imuSampleTime_ms;
+    hgtFailTime = imuSampleTime_ms;
+    lastStateStoreTime_ms = imuSampleTime_ms;
+    lastFixTime_ms = imuSampleTime_ms;
+    secondLastFixTime_ms = imuSampleTime_ms;
+    lastDecayTime_ms = imuSampleTime_ms;
+
     velTimeout = false;
     posTimeout = false;
     hgtTimeout = false;
     filterDiverged = false;
     magTimeout = false;
     magFailed = false;
-    lastHealthyMagTime_ms = hal.scheduler->millis();
-    lastStateStoreTime_ms = 0;
-    lastFixTime_ms = 0;
-    secondLastFixTime_ms = 0;
-    lastMagUpdate = 0;
-    lastAirspeedUpdate = 0;
-    velFailTime = 0;
-    posFailTime = 0;
-    hgtFailTime = 0;
     storeIndex = 0;
-    TASmsecPrev = 0;
-    BETAmsecPrev = 0;
-    MAGmsecPrev = 0;
-    HGTmsecPrev = 0;
-    lastMagUpdate = 0;
-    lastAirspeedUpdate = 0;
-    lastHgtMeasTime = 0;
     dtIMU = 0;
     dt = 0;
     hgtMea = 0;
@@ -3385,8 +3386,8 @@ bool NavEKF::use_compass(void) const
 // apply glitch offset to GPS measurements
 void NavEKF::decayGpsOffset()
 {
-    float lapsedTime = 0.001f*float(hal.scheduler->millis() - lastDecayTime_ms);
-    lastDecayTime_ms = hal.scheduler->millis();
+    float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms);
+    lastDecayTime_ms = imuSampleTime_ms;
     float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y);
     // decay radius if larger than velocity of 1.0 multiplied by lapsed time (plus a margin to prevent divide by zero)
     if (offsetRadius > (lapsedTime + 0.1f)) {
@@ -3422,11 +3423,11 @@ void NavEKF::checkDivergence()
     }
     bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f);
     lastGyroBias = state.gyro_bias;
-    if (hal.scheduler->millis() - lastDivergeTime_ms > 10000) {
+    if (imuSampleTime_ms - lastDivergeTime_ms > 10000) {
         if (divergenceDetected) {
             filterDiverged = true;
             faultStatus.diverged = true;
-            lastDivergeTime_ms = hal.scheduler->millis();
+            lastDivergeTime_ms = imuSampleTime_ms;
         } else {
             filterDiverged = false;
         }

libraries/AP_NavEKF/AP_NavEKF.h

@@ -431,7 +431,7 @@ private:
     Vector3f velDotNED;             // rate of change of velocity in NED frame
     Vector3f velDotNEDfilt;         // low pass filtered velDotNED
     uint32_t lastAirspeedUpdate;    // last time airspeed was updated
-    uint32_t IMUmsec;               // time that the last IMU value was taken
+    uint32_t imuSampleTime_ms;      // time that the last IMU value was taken
     ftype gpsCourse;                // GPS ground course angle(rad) 
     ftype gpsGndSpd;                // GPS ground speed (m/s)
     bool newDataGps;                // true when new GPS data has arrived