diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
index e384357d60..8aba16833c 100644
--- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
+++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
@@ -145,10 +145,16 @@ static THD_FUNCTION(main_loop,arg)
         g_callbacks->loop();
 
         /*
-          give up 250 microseconds of time, to ensure drivers get a
-          chance to run.
+          give up 250 microseconds of time if the INS loop hasn't
+          called delay_microseconds_boost(), to ensure low priority
+          drivers get a chance to run. Calling
+          delay_microseconds_boost() means we have already given up
+          time from the main loop, so we don't need to do it again
+          here
          */
-        hal.scheduler->delay_microseconds(250);
+        if (!schedulerInstance.check_called_boost()) {
+            hal.scheduler->delay_microseconds(250);
+        }
     }
     thread_running = false;
 }
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 077fad73bc..a2a8d36112 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -132,10 +132,22 @@ static void set_normal_priority()
  */
 void Scheduler::delay_microseconds_boost(uint16_t usec)
 {
-    delay_microseconds(usec); //Suspends Thread for desired microseconds
     set_high_priority();
-    delay_microseconds(APM_MAIN_PRIORITY_BOOST_USEC);
+    delay_microseconds(usec); //Suspends Thread for desired microseconds
     set_normal_priority();
+    _called_boost = true;
+}
+
+/*
+  return true if delay_microseconds_boost() has been called since last check
+ */
+bool Scheduler::check_called_boost(void)
+{
+    if (!_called_boost) {
+        return false;
+    }
+    _called_boost = false;
+    return true;
 }
 
 void Scheduler::delay(uint16_t ms)
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h
index b3ce71ec4e..bf2a6dc476 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.h
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.h
@@ -49,21 +49,6 @@
 #define APM_I2C_PRIORITY        176
 #endif
 
-/* 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: */
@@ -89,13 +74,16 @@ public:
     void     system_initialized();
     void     hal_initialized() { _hal_initialized = true; }
 
+    bool     check_called_boost(void);
+    
 private:
     bool _initialized;
     volatile bool _hal_initialized;
     AP_HAL::Proc _delay_cb;
     uint16_t _min_delay_cb_ms;
     AP_HAL::Proc _failsafe;
-
+    bool _called_boost;
+    
     volatile bool _timer_suspended;
 
     AP_HAL::MemberProc _timer_proc[CHIBIOS_SCHEDULER_MAX_TIMER_PROCS];