HAL_PX4: added delay_microseconds_boost()

implemented using hrt callback with sem_post wrapper

libraries/AP_HAL_PX4/HAL_PX4_Class.cpp

@@ -93,7 +93,7 @@ extern const AP_HAL::HAL& hal;
 /*
   set the priority of the main APM task
  */
-static void set_priority(uint8_t priority)
+void hal_px4_set_priority(uint8_t priority)
 {
     struct sched_param param;
     param.sched_priority = priority;
@@ -108,7 +108,7 @@ static void set_priority(uint8_t priority)
  */
 static void loop_overtime(void *)
 {
-    set_priority(APM_OVERTIME_PRIORITY);
+    hal_px4_set_priority(APM_OVERTIME_PRIORITY);
     px4_ran_overtime = true;
 }
 
@@ -134,7 +134,7 @@ static int main_loop(int argc, char **argv)
       run setup() at low priority to ensure CLI doesn't hang the
       system, and to allow initial sensor read loops to run
      */
-    set_priority(APM_STARTUP_PRIORITY);
+    hal_px4_set_priority(APM_STARTUP_PRIORITY);
 
     schedulerInstance.hal_initialized();
 
@@ -150,7 +150,7 @@ static int main_loop(int argc, char **argv)
     /*
       switch to high priority for main loop
      */
-    set_priority(APM_MAIN_PRIORITY);
+    hal_px4_set_priority(APM_MAIN_PRIORITY);
 
     while (!_px4_thread_should_exit) {
         perf_begin(perf_loop);
@@ -170,7 +170,7 @@ static int main_loop(int argc, char **argv)
               we ran over 1s in loop(), and our priority was lowered
               to let a driver run. Set it back to high priority now.
              */
-            set_priority(APM_MAIN_PRIORITY);
+            hal_px4_set_priority(APM_MAIN_PRIORITY);
             perf_count(perf_overrun);
             px4_ran_overtime = false;
         }

libraries/AP_HAL_PX4/HAL_PX4_Class.h

@@ -17,6 +17,8 @@ public:
     void init(int argc, char * const argv[]) const;
 };
 
+void hal_px4_set_priority(uint8_t priority);
+
 extern const HAL_PX4 AP_HAL_PX4;
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

libraries/AP_HAL_PX4/Scheduler.cpp

@@ -114,6 +114,39 @@ void PX4Scheduler::delay_microseconds(uint16_t usec)
     perf_end(_perf_delay);
 }
 
+/*
+  wrapper around sem_post that boosts main thread priority
+ */
+static void sem_post_boost(sem_t *sem)
+{
+    hal_px4_set_priority(APM_MAIN_PRIORITY_BOOST);
+    sem_post(sem);
+}
+
+/*
+  return the main thread to normal priority
+ */
+static void set_normal_priority(void *sem)
+{
+    hal_px4_set_priority(APM_MAIN_PRIORITY);
+}
+
+/*
+  a varient of delay_microseconds that boosts priority to
+  APM_MAIN_PRIORITY_BOOST for APM_MAIN_PRIORITY_BOOST_USEC
+  microseconds when the time completes. This significantly improves
+  the regularity of timing of the main loop as it takes 
+ */
+void PX4Scheduler::delay_microseconds_boost(uint16_t usec) 
+{
+    sem_t wait_semaphore;
+    static struct hrt_call wait_call;
+    sem_init(&wait_semaphore, 0, 0);
+    hrt_call_after(&wait_call, usec, (hrt_callout)sem_post_boost, &wait_semaphore);
+    sem_wait(&wait_semaphore);
+    hrt_call_after(&wait_call, APM_MAIN_PRIORITY_BOOST_USEC, (hrt_callout)set_normal_priority, NULL);
+}
+
 void PX4Scheduler::delay(uint16_t ms)
 {
     if (in_timerprocess()) {

libraries/AP_HAL_PX4/Scheduler.h

@@ -12,6 +12,7 @@
 
 #define PX4_SCHEDULER_MAX_TIMER_PROCS 8
 
+#define APM_MAIN_PRIORITY_BOOST 241
 #define APM_MAIN_PRIORITY       180
 #define APM_TIMER_PRIORITY      181
 #define APM_UART_PRIORITY        60
@@ -19,6 +20,21 @@
 #define APM_OVERTIME_PRIORITY    10
 #define APM_STARTUP_PRIORITY     10
 
+/* how long to boost priority of the main thread for each main
+   loop. This needs to be long enough for all interrupt-level drivers
+   (mostly SPI drivers) to run, and for the main loop of the vehicle
+   code to start the AHRS update.
+
+   Priority boosting of the main thread in delay_microseconds_boost()
+   avoids the problem that drivers in hpwork all happen to run right
+   at the start of the period where the main vehicle loop is calling
+   wait_for_sample(). That causes main loop timing jitter, which
+   reduces performance. Using the priority boost the main loop
+   temporarily runs at a priority higher than hpwork and the timer
+   thread, which results in much more consistent loop timing. 
+*/
+#define APM_MAIN_PRIORITY_BOOST_USEC 150
+
 #define APM_MAIN_THREAD_STACK_SIZE 8192
 
 /* Scheduler implementation: */
@@ -34,6 +50,7 @@ public:
     uint64_t millis64();
     uint64_t micros64();
     void     delay_microseconds(uint16_t us);
+    void     delay_microseconds_boost(uint16_t us);
     void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
     void     register_timer_process(AP_HAL::MemberProc);
     void     register_io_process(AP_HAL::MemberProc);