diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 36637c0987..76560422a7 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -29,6 +29,7 @@
 
 #include <AP_Scheduler/AP_Scheduler.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include "shared_dma.h"
 
 using namespace ChibiOS;
 
@@ -207,6 +208,10 @@ void Scheduler::reboot(bool hold_in_bootloader)
     hal.rcout->force_safety_on();
     hal.rcout->force_safety_no_wait();
 
+    // lock all shared DMA channels. This has the effect of waiting
+    // till the sensor buses are idle
+    Shared_DMA::lock_all();
+    
     // delay to ensure the async force_saftey operation completes
     delay(500);
 
diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.cpp b/libraries/AP_HAL_ChibiOS/shared_dma.cpp
index a381edb0d2..65aa0c1eb0 100644
--- a/libraries/AP_HAL_ChibiOS/shared_dma.cpp
+++ b/libraries/AP_HAL_ChibiOS/shared_dma.cpp
@@ -136,3 +136,14 @@ void Shared_DMA::unlock_from_IRQ(void)
     have_lock = false;
     chSysUnlockFromISR();
 }
+
+/*
+  lock all channels - used on reboot to ensure no sensor DMA is in
+  progress
+ */
+void Shared_DMA::lock_all(void)
+{
+    for (uint8_t i=0; i<SHARED_DMA_MAX_STREAM_ID; i++) {
+        chBSemWait(&locks[i].semaphore);
+    }
+}
diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.h b/libraries/AP_HAL_ChibiOS/shared_dma.h
index 54f5a13e92..c9d73ba9a6 100644
--- a/libraries/AP_HAL_ChibiOS/shared_dma.h
+++ b/libraries/AP_HAL_ChibiOS/shared_dma.h
@@ -55,6 +55,9 @@ public:
     //should be called inside the destructor of Shared DMA participants
     void unregister(void);
 
+    // lock all shared DMA channels. Used on reboot
+    static void lock_all(void);
+    
 private:
     dma_allocate_fn_t allocate;
     dma_allocate_fn_t deallocate;