From bc3b6fcb9ff50eeaa035ec331b9c05e27fa9d216 Mon Sep 17 00:00:00 2001
From: Pat Hickey <pat@galois.com>
Date: Thu, 3 Jan 2013 17:32:52 -0800
Subject: [PATCH] AP_HAL_AVR_SITL: remove begin/end atomic, adjust timer procs

---
 libraries/AP_HAL_AVR_SITL/SITL_State.cpp | 12 +++++------
 libraries/AP_HAL_AVR_SITL/Scheduler.cpp  | 26 +++++++++++++-----------
 libraries/AP_HAL_AVR_SITL/Scheduler.h    | 13 ++++++++----
 3 files changed, 29 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
index 0bd4c48124..7a6e901322 100644
--- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
@@ -193,11 +193,11 @@ void SITL_State::_timer_handler(int signum)
 
 	static bool in_timer;
 
-	if (in_timer || ((SITLScheduler *)hal.scheduler)->interrupts_are_blocked()) {
+	if (in_timer || _scheduler->interrupts_are_blocked()){
 		return;
-	}
-	_scheduler->begin_atomic();
+    }
 
+    _scheduler->sitl_begin_atomic();
 	in_timer = true;
 
 #ifndef __CYGWIN__
@@ -233,14 +233,14 @@ void SITL_State::_timer_handler(int signum)
 	if (_update_count == 0 && _sitl != NULL) {
 		_update_gps(0, 0, 0, 0, 0, false);
 		_scheduler->timer_event();
-		_scheduler->end_atomic();
+        _scheduler->sitl_end_atomic();
 		in_timer = false;
 		return;
 	}
 
 	if (_update_count == last_update_count) {
 		_scheduler->timer_event();
-		_scheduler->end_atomic();
+        _scheduler->sitl_end_atomic();
 		in_timer = false;
 		return;
 	}
@@ -262,7 +262,7 @@ void SITL_State::_timer_handler(int signum)
 	// interrupts, which can lead to recursion
 	_scheduler->timer_event();
 
-	_scheduler->end_atomic();
+    _scheduler->sitl_end_atomic();
 	in_timer = false;
 }
 
diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
index 085d847ffd..62fa7ae6f4 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
+++ b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp
@@ -15,6 +15,7 @@ extern const AP_HAL::HAL& hal;
 
 AP_HAL::TimedProc SITLScheduler::_failsafe = NULL;
 volatile bool SITLScheduler::_timer_suspended = false;
+volatile bool SITLScheduler::_timer_event_missed = false;
 AP_HAL::TimedProc SITLScheduler::_timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
 uint8_t SITLScheduler::_num_timer_procs = 0;
 bool SITLScheduler::_in_timer_proc = false;
@@ -110,26 +111,25 @@ void SITLScheduler::suspend_timer_procs() {
 
 void SITLScheduler::resume_timer_procs() {
     _timer_suspended = false;
-}
-
-void SITLScheduler::begin_atomic() {
-    _nested_atomic_ctr++;
-}
-
-void SITLScheduler::end_atomic() {
-	if (_nested_atomic_ctr == 0) {
-        hal.uartA->println_P(PSTR("ATOMIC NESTING ERROR"));
-        return;
+    if (_timer_event_missed) {
+        _timer_event_missed = false;
+        _run_timer_procs(false);
     }
-    _nested_atomic_ctr--;
 }
 
+void SITLScheduler::sitl_end_atomic() {
+    if (_nested_atomic_ctr == 0) 
+        hal.uartA->println_P(PSTR("NESTED ATOMIC ERROR"));
+    else
+        _nested_atomic_ctr--;
+}   
+
 void SITLScheduler::reboot() 
 {
     hal.uartA->println_P(PSTR("REBOOT NOT IMPLEMENTED\r\n"));
 }
 
-void SITLScheduler::timer_event() 
+void SITLScheduler::_run_timer_procs(bool called_from_isr) 
 {
     uint32_t tnow = _micros();
     if (_in_timer_proc) {
@@ -157,6 +157,8 @@ void SITLScheduler::timer_event()
                 _timer_proc[i](tnow);
             }
         }
+    } else if (called_from_isr) {
+        _timer_event_missed = true;
     }
 
     // and the failsafe, if one is setup
diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.h b/libraries/AP_HAL_AVR_SITL/Scheduler.h
index c7d82011ba..4931ec862e 100644
--- a/libraries/AP_HAL_AVR_SITL/Scheduler.h
+++ b/libraries/AP_HAL_AVR_SITL/Scheduler.h
@@ -21,21 +21,23 @@ public:
     uint32_t micros();
     void     delay_microseconds(uint16_t us);
     void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
+
     void     register_timer_process(AP_HAL::TimedProc);
-    void     register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us);
     void     suspend_timer_procs();
     void     resume_timer_procs();
-    void     begin_atomic();
-    void     end_atomic();
+
+    void     register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us);
     void     reboot();
     void     panic(const prog_char_t *errormsg);
 
     bool     interrupts_are_blocked(void) { return _nested_atomic_ctr != 0; }
 
-    static void timer_event(void);
+    void     sitl_begin_atomic() { _nested_atomic_ctr++; }
+    void     sitl_end_atomic();
 
     // callable from interrupt handler
     static uint32_t _micros();
+    static void timer_event() { _run_timer_procs(true); }
 
 private:
     uint8_t _nested_atomic_ctr;
@@ -44,7 +46,10 @@ private:
     static struct timeval _sketch_start_time;
     static AP_HAL::TimedProc _failsafe;
 
+    static void _run_timer_procs(bool called_from_isr);
+
     static volatile bool _timer_suspended;
+    static volatile bool _timer_event_missed;
     static AP_HAL::TimedProc _timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS];
     static uint8_t _num_timer_procs;
     static bool    _in_timer_proc;