From d47f77f8dd8925f335cb4b020a1cbd053f0a1a38 Mon Sep 17 00:00:00 2001
From: Pat Hickey <pat@galois.com>
Date: Mon, 3 Dec 2012 11:03:53 -0800
Subject: [PATCH] AP_HAL_AVR Scheduler: support nested atomic sections

---
 libraries/AP_HAL_AVR/Scheduler.cpp | 10 ++++++++--
 libraries/AP_HAL_AVR/Scheduler.h   |  2 ++
 2 files changed, 10 insertions(+), 2 deletions(-)

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__