AP_HAL_AVR: deprecate begin/end atomic, timer procs run on resume

libraries/AP_HAL_AVR/Scheduler.cpp

@@ -22,6 +22,7 @@ AVRTimer AVRScheduler::_timer;
 
 AP_HAL::TimedProc AVRScheduler::_failsafe = NULL;
 volatile bool AVRScheduler::_timer_suspended = false;
+volatile bool AVRScheduler::_timer_event_missed = false;
 volatile bool AVRScheduler::_in_timer_proc = false;
 AP_HAL::TimedProc AVRScheduler::_timer_proc[AVR_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
 uint8_t AVRScheduler::_num_timer_procs = 0;
@@ -29,8 +30,7 @@ uint8_t AVRScheduler::_num_timer_procs = 0;
 
 AVRScheduler::AVRScheduler() :
     _delay_cb(NULL),
-    _min_delay_cb_ms(65535),
-    _nested_atomic_ctr(0)
+    _min_delay_cb_ms(65535)
 {}
 
 void AVRScheduler::init(void* _isrregistry) {
@@ -47,8 +47,8 @@ void AVRScheduler::init(void* _isrregistry) {
     TCNT2 = 0;                      /* Set count to 0 */
     TIFR2 = _BV(TOV2);              /* Clear pending interrupts */
     TIMSK2 = _BV(TOIE2);            /* Enable overflow interrupt*/
-    /* Register _timer_event to trigger on overflow */
-    isrregistry->register_signal(ISR_REGISTRY_TIMER2_OVF, _timer_event);   
+    /* Register _timer_isr_event to trigger on overflow */
+    isrregistry->register_signal(ISR_REGISTRY_TIMER2_OVF, _timer_isr_event);   
 }
 
 uint32_t AVRScheduler::micros() {
@@ -119,9 +119,13 @@ void AVRScheduler::suspend_timer_procs() {
 
 void AVRScheduler::resume_timer_procs() {
     _timer_suspended = false;
+    if (_timer_event_missed == true) {
+        _run_timer_procs(false);
+        _timer_event_missed = false;
+    }
 }
 
-void AVRScheduler::_timer_event() {
+void AVRScheduler::_timer_isr_event() {
     // we enable the interrupt again immediately and also enable
     // interrupts. This allows other time critical interrupts to
     // run (such as the serial receive interrupt). We catch the
@@ -131,6 +135,10 @@ void AVRScheduler::_timer_event() {
 
     TCNT2 = RESET_TCNT2_VALUE;
     sei();
+    _run_timer_procs(true);
+}
+
+void AVRScheduler::_run_timer_procs(bool called_from_isr) {
 
     uint32_t tnow = _timer.micros();
     if (_in_timer_proc) {
@@ -149,6 +157,7 @@ void AVRScheduler::_timer_event() {
         }
         return;
     }
+
     _in_timer_proc = true;
 
     if (!_timer_suspended) {
@@ -158,6 +167,8 @@ void AVRScheduler::_timer_event() {
                 _timer_proc[i](tnow);
             }
         }
+    } else if (called_from_isr) {
+        _timer_event_missed = true;
     }
 
     // and the failsafe, if one is setup
@@ -168,22 +179,6 @@ void AVRScheduler::_timer_event() {
     _in_timer_proc = false;
 }
 
-void AVRScheduler::begin_atomic() {
-    _nested_atomic_ctr++;
-    cli();
-}
-
-void AVRScheduler::end_atomic() {
-	if (_nested_atomic_ctr == 0) {
-        hal.uartA->println_P(PSTR("ATOMIC NESTING ERROR"));
-        return;
-    }
-    _nested_atomic_ctr--;
-    if (_nested_atomic_ctr == 0) {
-        sei();
-    }
-}
-
 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. */

libraries/AP_HAL_AVR/Scheduler.h

@@ -32,12 +32,12 @@ 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     panic(const prog_char_t *errormsg);
     void     reboot();
 
@@ -47,21 +47,21 @@ protected:
 private:
     static AVRTimer _timer;
 
-    /* timer_event() is static so it can be called from an interrupt.
-     * (This is effectively a singleton class.)
-     * _prefix: this method must be public */
-    static void _timer_event();
-
     AP_HAL::Proc _delay_cb;
     uint16_t _min_delay_cb_ms;
+
+    /* _timer_isr_event() and _run_timer_procs are static so they can be
+     * called from an interrupt. */
+    static void _timer_isr_event();
+    static void _run_timer_procs(bool called_from_isr);
+
     static AP_HAL::TimedProc _failsafe;
 
     static volatile bool _timer_suspended;
+    static volatile bool _timer_event_missed;
     static AP_HAL::TimedProc _timer_proc[AVR_SCHEDULER_MAX_TIMER_PROCS];
     static uint8_t _num_timer_procs;
 
-    uint8_t _nested_atomic_ctr;
-
 };
 #endif // __AP_HAL_AVR_SCHEDULER_H__
 

libraries/AP_HAL_AVR/Semaphores.cpp

@@ -1,4 +1,10 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_HAL.h>
+
+#if (CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2)
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
 
 #include <AP_HAL.h>
 #include <AP_HAL_AVR.h>
@@ -60,12 +66,14 @@ bool AVRSemaphore::_take_from_mainloop(uint32_t timeout_ms) {
 
 bool AVRSemaphore::_take_nonblocking() {
     bool result = false;
-    hal.scheduler->begin_atomic();
+    uint8_t sreg = SREG;
+    cli();
     if (!_taken) {
         _taken = true;
         result = true;
     }
-    hal.scheduler->end_atomic();
+    SREG = sreg;
     return result;
 }
 
+#endif // CONFIG_HAL_BOARD