From 4756dbee844e4a9c44d9f06c8d3a1658f73487d3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 21 Apr 2014 13:16:20 +1000
Subject: [PATCH] AP_NavEKF: fixed millisecond subtraction for rollover

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 0841e6eaf1..77c6f23609 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -654,12 +654,14 @@ void NavEKF::SelectVelPosFusion()
             // reset the counter used to schedule updates so that we always fuse data on the frame GPS data arrives
             skipCounter = velPosFuseStepRatio;
             // If a long time since last GPS update, then reset position and velocity and reset stored state history
-            if ((hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeUseTAS && useAirspeed()) || (hal.scheduler->millis() > secondLastFixTime_ms + _gpsRetryTimeNoTAS && !useAirspeed())) {
-            ResetPosition();
-            ResetVelocity();
-            StoreStatesReset();
+
+            uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS;
+            if (hal.scheduler->millis() - secondLastFixTime_ms > gpsRetryTimeout) {
+                ResetPosition();
+                ResetVelocity();
+                StoreStatesReset();
             }
-        } else if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) {
+        } else if (hal.scheduler->millis() - lastFixTime_ms > _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;
@@ -673,7 +675,7 @@ void NavEKF::SelectVelPosFusion()
             newDataHgt = false;
             // enable fusion
             fuseHgtData = true;
-        } else if (hal.scheduler->millis() > lastHgtTime_ms + _msecHgtAvg + 40) {
+        } else if (hal.scheduler->millis() - lastHgtTime_ms > _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;