diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp
index 0c0c4de1a5..ac8db44413 100644
--- a/libraries/AP_HAL_AVR/Scheduler.cpp
+++ b/libraries/AP_HAL_AVR/Scheduler.cpp
@@ -27,7 +27,9 @@ bool AVRScheduler::_in_timer_proc = false;
 
 
 AVRScheduler::AVRScheduler() :
-    _delay_cb(NULL)
+    _delay_cb(NULL),
+    _min_delay_cb_ms(65535),
+    _nested_atomic_ctr(0)
 {}
 
 void AVRScheduler::init(void* _isrregistry) {
@@ -323,9 +325,13 @@ void AVRScheduler::_timer_event() {
 }
 
 void AVRScheduler::begin_atomic() {
+    _nested_atomic_ctr++;
     cli();
 }
 
 void AVRScheduler::end_atomic() {
-    sei();
+    _nested_atomic_ctr--;
+    if (_nested_atomic_ctr == 0) {
+        sei();
+    }
 }
diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h
index 5af887cb99..1f779841c1 100644
--- a/libraries/AP_HAL_AVR/Scheduler.h
+++ b/libraries/AP_HAL_AVR/Scheduler.h
@@ -49,6 +49,8 @@ private:
     static uint8_t _num_timer_procs;
     static bool    _in_timer_proc;
 
+    uint8_t _nested_atomic_ctr;
+
 };
 #endif // __AP_HAL_AVR_SCHEDULER_H__