From 5a55101703bb8e41cc84aef80fe3164e64e0d791 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 20 Aug 2014 07:57:45 +1000
Subject: [PATCH] HAL_SITL: added millis64() and micros64()

---
 libraries/AP_HAL_AVR_SITL/Scheduler.cpp | 54 +++++++++++++++----------
 libraries/AP_HAL_AVR_SITL/Scheduler.h   |  4 +-
 libraries/AP_HAL_AVR_SITL/sitl_ins.cpp  |  2 +-
 3 files changed, 37 insertions(+), 23 deletions(-)

diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
index db0d172824..6d7a8a2024 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
+++ b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
@@ -70,52 +70,64 @@ double SITLScheduler::_cyg_sec()
 }
 #endif
 
-uint32_t SITLScheduler::_micros() 
+uint64_t SITLScheduler::_micros64() 
 {
 #ifdef __CYGWIN__
-	return (uint32_t)(_cyg_sec() * 1.0e6);
+	return (uint64_t)(_cyg_sec() * 1.0e6);
 #else   
 	struct timeval tp;
 	gettimeofday(&tp,NULL);
-	return 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
-		      (_sketch_start_time.tv_sec +
-		       (_sketch_start_time.tv_usec*1.0e-6)));
+	uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
+                          (_sketch_start_time.tv_sec +
+                           (_sketch_start_time.tv_usec*1.0e-6)));
+    return ret;
 #endif
 }
 
+uint64_t SITLScheduler::micros64() 
+{
+    return _micros64();
+}
+
 uint32_t SITLScheduler::micros() 
 {
-    return _micros();
+    return micros64() & 0xFFFFFFFF;
+}
+
+uint64_t SITLScheduler::millis64() 
+{
+#ifdef __CYGWIN__
+	// 1000 ms in a second
+	return (uint64_t)(_cyg_sec() * 1000);
+#else
+	struct timeval tp;
+	gettimeofday(&tp,NULL);
+	uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
+                          (_sketch_start_time.tv_sec +
+                           (_sketch_start_time.tv_usec*1.0e-6)));
+    return ret;
+#endif
 }
 
 uint32_t SITLScheduler::millis() 
 {
-#ifdef __CYGWIN__
-	// 1000 ms in a second
-	return (uint32_t)(_cyg_sec() * 1000);
-#else
-	struct timeval tp;
-	gettimeofday(&tp,NULL);
-	return 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - 
-		      (_sketch_start_time.tv_sec +
-		       (_sketch_start_time.tv_usec*1.0e-6)));
-#endif
+    return millis64() & 0xFFFFFFFF;
 }
 
 void SITLScheduler::delay_microseconds(uint16_t usec) 
 {
-	uint32_t start = micros();
-	while (micros() - start < usec) {
-		usleep(usec - (micros() - start));
+	uint64_t start = micros64();
+	while (micros64() - start < usec) {
+		usleep(usec - (micros64() - start));
 	}
 }
 
 void SITLScheduler::delay(uint16_t ms)
 {
-	uint32_t start = micros();
+	uint64_t start = micros64();
     
     while (ms > 0) {
-        while ((micros() - start) >= 1000) {
+        while ((micros64() - start) >= 1000) {
             ms--;
             if (ms == 0) break;
             start += 1000;
diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.h b/libraries/AP_HAL_AVR_SITL/Scheduler.h
index bd6e33aba9..8719e723e0 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.h
+++ b/libraries/AP_HAL_AVR_SITL/Scheduler.h
@@ -19,6 +19,8 @@ public:
     void     delay(uint16_t ms);
     uint32_t millis();
     uint32_t micros();
+    uint64_t millis64();
+    uint64_t micros64();
     void     delay_microseconds(uint16_t us);
     void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
 
@@ -43,7 +45,7 @@ public:
     void     sitl_end_atomic();
 
     // callable from interrupt handler
-    static uint32_t _micros();
+    static uint64_t _micros64();
     static void timer_event() { _run_timer_procs(true); _run_io_procs(true); }
 
 private:
diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
index 06714a84c1..71875995a7 100644
--- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
+++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
@@ -52,7 +52,7 @@ float SITL_State::_gyro_drift(void)
 		return 0;
 	}
 	double period  = _sitl->drift_time * 2;
-	double minutes = fmod(_scheduler->_micros() / 60.0e6, period);
+	double minutes = fmod(_scheduler->_micros64() / 60.0e6, period);
 	if (minutes < period/2) {
 		return minutes * ToRad(_sitl->drift_speed);
 	}