From b9dc2335eea186c9f79bc157ed67501dc6105048 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 19 Aug 2014 20:03:15 +1000
Subject: [PATCH] HAL_Linux: run timer thread at average 1kHz without drift

this avoids drift in the 1kHz timer, to avoid bias in the IMU
filtering
---
 libraries/AP_HAL_Linux/Scheduler.cpp | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp
index d0d1760d15..9644c7f0a5 100644
--- a/libraries/AP_HAL_Linux/Scheduler.cpp
+++ b/libraries/AP_HAL_Linux/Scheduler.cpp
@@ -235,12 +235,22 @@ void *LinuxScheduler::_timer_thread(void)
     while (system_initializing()) {
         poll(NULL, 0, 1);        
     }
+    /*
+      this aims to run at an average of 1kHz, so that it can be used
+      to drive 1kHz processes without drift
+     */
+    uint32_t next_run_usec = micros() + 1000;
     while (true) {
-        _microsleep(1000);
-
+        uint32_t dt = next_run_usec - micros();
+        if (dt > 2000) {
+            // we've lost sync - restart
+            next_run_usec = micros();
+        } else {
+            _microsleep(dt);
+        }
+        next_run_usec += 1000;
         // run registered timers
         _run_timers(true);
-
     }
     return NULL;
 }