diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp
index bfc90b5777..d95e22cf22 100644
--- a/libraries/AP_HAL_PX4/Scheduler.cpp
+++ b/libraries/AP_HAL_PX4/Scheduler.cpp
@@ -164,6 +164,12 @@ void PX4Scheduler::resume_timer_procs() {
 
 void PX4Scheduler::begin_atomic() {
     _nested_atomic_ctr++;
+    if (_nested_atomic_ctr == 1) {
+        sigset_t set;
+        sigemptyset(&set);
+        sigaddset(&set, MAIN_TIMER_SIGNAL);
+        sigprocmask(SIG_BLOCK, &set, NULL);
+    }
 }
 
 void PX4Scheduler::end_atomic() {
@@ -172,6 +178,12 @@ void PX4Scheduler::end_atomic() {
         return;
     }
     _nested_atomic_ctr--;
+    if (_nested_atomic_ctr == 0) {
+        sigset_t set;
+        sigemptyset(&set);
+        sigaddset(&set, MAIN_TIMER_SIGNAL);
+        sigprocmask(SIG_UNBLOCK, &set, NULL);
+    }
 }
 
 void PX4Scheduler::reboot()