AP_NavEKF2: Fix timer wrapping bugs

Also replaces tab characters

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -240,8 +240,8 @@ void NavEKF2_core::setAidingMode()
             }
             posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
                    (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
-            posAidLossPending = (imuSampleTime_ms > lastRngBcnPassTime_ms + frontend->_gsfResetDelay) &&
-                    (imuSampleTime_ms > lastPosPassTime_ms + frontend->_gsfResetDelay);
+            posAidLossPending = (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->_gsfResetDelay) &&
+                    (imuSampleTime_ms - lastPosPassTime_ms > frontend->_gsfResetDelay);
         }
 
         if (attAidLossCritical) {
@@ -544,7 +544,7 @@ void NavEKF2_core::runYawEstimator()
     if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
         float trueAirspeed;
         if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) {
-            if (imuDataDelayed.time_ms < (tasDataDelayed.time_ms + 5000)) {
+            if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
                 trueAirspeed = tasDataDelayed.tas;
             } else {
                 trueAirspeed = defaultAirSpeed * AP::ahrs().get_EAS2TAS();
@@ -562,7 +562,7 @@ void NavEKF2_core::runYawEstimator()
         }
 
         // action an external reset request
-        if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms < (EKFGSF_yaw_reset_request_ms + YAW_RESET_TO_GSF_TIMEOUT_MS)) {
+        if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS) {
             EKFGSF_resetMainFilterYaw();
         }
     }

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -838,7 +838,7 @@ void NavEKF2_core::fuseEulerYaw()
             extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
             measured_yaw =  euler321.z;
         } else {
-            if (imuSampleTime_ms > prevBetaStep_ms + 1000 && yawEstimator != nullptr) {
+            if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
                 float gsfYaw, gsfYawVariance;
                 if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) &&
                     is_positive(gsfYawVariance) &&
@@ -897,7 +897,7 @@ void NavEKF2_core::fuseEulerYaw()
             euler312 = extNavDataDelayed.quat.to_vector312();
             measured_yaw =  euler312.z;
         } else {
-            if (imuSampleTime_ms > prevBetaStep_ms + 1000 && yawEstimator != nullptr) {
+            if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
                 float gsfYaw, gsfYawVariance;
                 if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) &&
                     is_positive(gsfYawVariance) &&
@@ -1247,9 +1247,9 @@ bool NavEKF2_core::EKFGSF_resetMainFilterYaw()
         ResetVelocity();
         ResetPosition();
 
-		// reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset
-		velTestRatio = 0.0f;
-		posTestRatio = 0.0f;
+        // reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset
+        velTestRatio = 0.0f;
+        posTestRatio = 0.0f;
 
         return true;
 

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -1251,8 +1251,8 @@ private:
     float InitialGyroBiasUncertainty(void) const;
 
     // The following declarations are used to control when the main navigation filter resets it's yaw to the estimate provided by the GSF
-	uint64_t EKFGSF_yaw_reset_ms;	        // timestamp of last emergency yaw reset (uSec)
-	uint64_t EKFGSF_yaw_reset_request_ms;   // timestamp of last emergency yaw reset request (uSec)
+	uint32_t EKFGSF_yaw_reset_ms;	        // timestamp of last emergency yaw reset (uSec)
+	uint32_t EKFGSF_yaw_reset_request_ms;   // timestamp of last emergency yaw reset request (uSec)
     uint8_t EKFGSF_yaw_reset_count;         // number of emergency yaw resets performed
     bool EKFGSF_run_filterbank;             // true when the filter bank is active
 };