HAL_PX4: use hrt timer and semaphores to improve timing

this gives much more accurate microsecond delays, while also ensuring
we yield the CPU when possible

libraries/AP_HAL_PX4/HAL_PX4_Class.cpp

@@ -76,11 +76,6 @@ static bool ran_overtime;
 
 extern const AP_HAL::HAL& hal;
 
-static void semaphore_yield(void *sem)
-{
-    sem_post((sem_t *)sem);
-}
-
 /*
   set the priority of the main APM task
  */
@@ -130,12 +125,9 @@ static int main_loop(int argc, char **argv)
 
     perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");
     perf_counter_t perf_overrun = perf_alloc(PC_COUNT, "APM_overrun");
-    sem_t loop_semaphore;
     struct hrt_call loop_call;
     struct hrt_call loop_overtime_call;
 
-    sem_init(&loop_semaphore, 0, 0);
-             
     thread_running = true;
 
     /*
@@ -179,11 +171,10 @@ static int main_loop(int argc, char **argv)
 
         /*
           give up 500 microseconds of time, to ensure drivers get a
-          chance to run. This gives us better timing performance than
-          a poll(NULL, 0, 1)
+          chance to run. This relies on the accurate semaphore wait
+          using hrt in semaphore.cpp
          */
-        hrt_call_after(&loop_call, 500, (hrt_callout)semaphore_yield, &loop_semaphore);
-        sem_wait(&loop_semaphore);
+        hal.scheduler->delay_microseconds(500);
     }
     thread_running = false;
     return 0;

libraries/AP_HAL_PX4/Scheduler.cpp

@@ -14,6 +14,7 @@
 #include <drivers/drv_hrt.h>
 #include <nuttx/arch.h>
 #include <systemlib/systemlib.h>
+#include <pthread.h>
 #include <poll.h>
 
 #include "UARTDriver.h"
@@ -82,15 +83,26 @@ uint32_t PX4Scheduler::millis()
     return hrt_absolute_time() / 1000;
 }
 
+/**
+   delay for a specified number of microseconds using a semaphore wait
+ */
+void PX4Scheduler::delay_microseconds_semaphore(uint16_t usec) 
+{
+    sem_t wait_semaphore;
+    struct hrt_call wait_call;
+    sem_init(&wait_semaphore, 0, 0);
+    hrt_call_after(&wait_call, usec, (hrt_callout)sem_post, &wait_semaphore);
+    sem_wait(&wait_semaphore);
+}
+
 void PX4Scheduler::delay_microseconds(uint16_t usec) 
 {
-#if 0
-    if (_in_timer_proc) {
-        ::printf("ERROR: delay_microseconds() from timer process\n");
+    perf_begin(_perf_delay);
+    if (usec >= 500) {
+        delay_microseconds_semaphore(usec);
+        perf_end(_perf_delay);
         return;
     }
-#endif
-    perf_begin(_perf_delay);
 	uint32_t start = micros();
     uint32_t dt;
 	while ((dt=(micros() - start)) < usec) {
@@ -112,8 +124,7 @@ void PX4Scheduler::delay(uint16_t ms)
     
     while ((hrt_absolute_time() - start)/1000 < ms && 
            !_px4_thread_should_exit) {
-        // this yields the CPU to other apps
-        poll(NULL, 0, 1);
+        delay_microseconds_semaphore(1000);
         if (_min_delay_cb_ms <= ms) {
             if (_delay_cb) {
                 _delay_cb();
@@ -221,7 +232,7 @@ void PX4Scheduler::_run_timers(bool called_from_timer_thread)
 void *PX4Scheduler::_timer_thread(void)
 {
     while (!_px4_thread_should_exit) {
-        poll(NULL, 0, 1);
+        delay_microseconds_semaphore(1000);
 
         // run registered timers
         perf_begin(_perf_timers);

libraries/AP_HAL_PX4/Scheduler.h

@@ -74,6 +74,8 @@ private:
     void _run_timers(bool called_from_timer_thread);
     void _run_io(void);
 
+    void delay_microseconds_semaphore(uint16_t us);
+
     perf_counter_t  _perf_timers;
     perf_counter_t  _perf_io_timers;
     perf_counter_t  _perf_delay;