From 9f423a24adb97f9804213fc41bc85b2d87f59a4d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 4 Jan 2013 11:49:32 +1100
Subject: [PATCH] HAL_PX4: yield CPU in delay()

this allows other apps to run
---
 libraries/AP_HAL_PX4/Scheduler.cpp | 12 +++++-------
 1 file changed, 5 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp
index 3f2a475799..3db495e3aa 100644
--- a/libraries/AP_HAL_PX4/Scheduler.cpp
+++ b/libraries/AP_HAL_PX4/Scheduler.cpp
@@ -14,6 +14,7 @@
 #include <drivers/drv_hrt.h>
 #include <nuttx/arch.h>
 #include <systemlib/systemlib.h>
+#include <poll.h>
 
 using namespace PX4;
 
@@ -66,14 +67,11 @@ void PX4Scheduler::delay_microseconds(uint16_t usec)
 
 void PX4Scheduler::delay(uint16_t ms)
 {
-	uint32_t start = micros();
+	uint64_t start = hrt_absolute_time();
     
-    while (ms > 0) {
-        while ((micros() - start) >= 1000) {
-            ms--;
-            if (ms == 0) break;
-            start += 1000;
-        }
+    while ((hrt_absolute_time() - start)/1000 < ms) {
+        // this yields the CPU to other apps
+        poll(NULL, 0, 1);
         if (_min_delay_cb_ms <= ms) {
             if (_delay_cb) {
                 _delay_cb();