From 930a789b258dbd62171e0615647a614e7ab27faa Mon Sep 17 00:00:00 2001
From: Pat Hickey <pat@galois.com>
Date: Thu, 10 Jan 2013 13:50:23 -0800
Subject: [PATCH] AP_HAL_AVR: Implement new scheduler methods

---
 libraries/AP_HAL_AVR/Scheduler.cpp  | 19 ++++++++++++++++++-
 libraries/AP_HAL_AVR/Scheduler.h    | 14 +++++++++-----
 libraries/AP_HAL_AVR/Semaphores.cpp |  4 ++--
 3 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp
index 4a95d4eecd..b2a7d93956 100644
--- a/libraries/AP_HAL_AVR/Scheduler.cpp
+++ b/libraries/AP_HAL_AVR/Scheduler.cpp
@@ -30,7 +30,8 @@ uint8_t AVRScheduler::_num_timer_procs = 0;
 
 AVRScheduler::AVRScheduler() :
     _delay_cb(NULL),
-    _min_delay_cb_ms(65535)
+    _min_delay_cb_ms(65535),
+    _initialized(false)
 {}
 
 void AVRScheduler::init(void* _isrregistry) {
@@ -125,6 +126,10 @@ void AVRScheduler::resume_timer_procs() {
     }
 }
 
+bool AVRScheduler::in_timerprocess() {
+    return _in_timer_proc;
+}
+
 void AVRScheduler::_timer_isr_event() {
     // we enable the interrupt again immediately and also enable
     // interrupts. This allows other time critical interrupts to
@@ -179,6 +184,18 @@ void AVRScheduler::_run_timer_procs(bool called_from_isr) {
     _in_timer_proc = false;
 }
 
+bool AVRScheduler::system_initializing() {
+    return !_initialized;
+}
+
+void AVRScheduler::system_initialized() {
+    if (_initialized) {
+        panic(PSTR("PANIC: scheduler::system_initialized called"
+                   "more than once"));
+    }
+    _initialized = true;
+}
+
 void AVRScheduler::panic(const prog_char_t* errormsg) {
     /* Suspend timer processes. We still want the timer event to go off
      * to run the _failsafe code, however. */
diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h
index 0eb8ff2fcd..52fa5920af 100644
--- a/libraries/AP_HAL_AVR/Scheduler.h
+++ b/libraries/AP_HAL_AVR/Scheduler.h
@@ -18,8 +18,6 @@ public:
 
 /* Scheduler implementation: */
 class AP_HAL_AVR::AVRScheduler : public AP_HAL::Scheduler {
-    /* AVRSemaphore gets access to _in_timer_proc */
-    friend class AVRSemaphore;
 public:
     AVRScheduler();
     /* AP_HAL::Scheduler methods */
@@ -37,18 +35,24 @@ public:
     void     suspend_timer_procs();
     void     resume_timer_procs();
 
+    bool     in_timerprocess();
+
     void     register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us);
+
+    bool     system_initializing();
+    void     system_initialized();
+
     void     panic(const prog_char_t *errormsg);
     void     reboot();
 
-protected:
-    static volatile bool _in_timer_proc;
-
 private:
     static AVRTimer _timer;
 
+    static volatile bool _in_timer_proc;
+
     AP_HAL::Proc _delay_cb;
     uint16_t _min_delay_cb_ms;
+    bool _initialized;
 
     /* _timer_isr_event() and _run_timer_procs are static so they can be
      * called from an interrupt. */
diff --git a/libraries/AP_HAL_AVR/Semaphores.cpp b/libraries/AP_HAL_AVR/Semaphores.cpp
index 116edf30b7..93f2c6fbd9 100644
--- a/libraries/AP_HAL_AVR/Semaphores.cpp
+++ b/libraries/AP_HAL_AVR/Semaphores.cpp
@@ -27,7 +27,7 @@ bool AVRSemaphore::give() {
 }
 
 bool AVRSemaphore::take(uint32_t timeout_ms) {
-    if (AVRScheduler::_in_timer_proc) {
+    if (hal.scheduler->in_timerprocess()) {
         hal.scheduler->panic(PSTR("PANIC: AVRSemaphore::take used from "
                     "inside timer process"));
         return false; /* Never reached - panic does not return */
@@ -36,7 +36,7 @@ bool AVRSemaphore::take(uint32_t timeout_ms) {
 }
 
 bool AVRSemaphore::take_nonblocking() {
-    if (AVRScheduler::_in_timer_proc) {
+    if (hal.scheduler->in_timerprocess()) {
         return _take_nonblocking();
     } else {
         return _take_from_mainloop(0);