AP_HAL_AVR: Scheduler extensions implemented

libraries/AP_HAL_AVR/HAL_AVR.cpp

@@ -4,7 +4,7 @@ using namespace AP_HAL_AVR;
 
 void HAL_AVR::init(void* opts) const {
 
-    scheduler->init();
+    scheduler->init((void*)&isr_registry);
    
     /* uart0 is the serial port used for the console, so lets make sure
      * it is initialized at boot */

libraries/AP_HAL_AVR/Scheduler.cpp

@@ -6,7 +6,6 @@ using namespace AP_HAL_AVR;
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
-
 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
 #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
 
@@ -14,7 +13,25 @@ static volatile uint32_t timer0_overflow_count = 0;
 static volatile uint32_t timer0_millis = 0;
 static uint8_t timer0_fract = 0;
 
-void ArduinoScheduler::init() {
+/* ArduinoScheduler timer interrupt period is controlled by TCNT2.
+ * 256-62 gives a 1kHz period. */
+#define RESET_TCNT2_VALUE (256 - 62)
+
+/* Static ArduinoScheduler variables: */
+AP_HAL::TimedProc ArduinoScheduler::_failsafe = NULL;
+volatile bool ArduinoScheduler::_timer_suspended = false;
+AP_HAL::TimedProc ArduinoScheduler::_timer_proc[AVR_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
+uint8_t ArduinoScheduler::_num_timer_procs = 0;
+bool ArduinoScheduler::_in_timer_proc = false;
+
+
+ArduinoScheduler::ArduinoScheduler() :
+    _delay_cb(NULL)
+{}
+
+void ArduinoScheduler::init(void* _isrregistry) {
+    ISRRegistry* isrregistry = (ISRRegistry*) _isrregistry;
+
     // this needs to be called before setup() or some functions won't
     // work there
     sei();
@@ -39,9 +56,6 @@ void ArduinoScheduler::init() {
     // put timer 1 in 8-bit phase correct pwm mode
     sbi(TCCR1A, WGM10);
 
-    // set timer 2 prescale factor to 64
-    sbi(TCCR2B, CS22);
-
     sbi(TCCR3B, CS31);      // set timer 3 prescale factor to 64
     sbi(TCCR3B, CS30);
     sbi(TCCR3A, WGM30);     // put timer 3 in 8-bit phase correct pwm mode
@@ -69,6 +83,16 @@ void ArduinoScheduler::init() {
     // here so they can be used as normal digital i/o; they will be
     // reconnected in Serial.begin()
     UCSR0B = 0;
+
+    /* TIMER2: Setup the overflow interrupt to occur at 1khz. */
+    TIMSK2 = 0;                     /* Disable timer interrupt */
+    TCCR2A = 0;                     /* Normal counting mode */
+    TCCR2B = _BV(CS21) | _BV(CS22); /* Prescaler to clk/256 */
+    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);   
 }
 
 #define clockCyclesPerMicrosecond() ( F_CPU / 1000000L )
@@ -121,7 +145,15 @@ uint32_t ArduinoScheduler::millis()
 	return m;
 }
 
+/* micros() is essentially a static method, but we need it to be available
+ * via virtual dispatch through the hal. */
 uint32_t ArduinoScheduler::micros() {
+    return _micros();
+}
+
+/* _micros() is the implementation of micros() as a static private method
+ * so we can use it from inside _timer_event() without virtual dispatch. */
+uint32_t ArduinoScheduler::_micros() {
 	uint32_t m;
 	uint8_t oldSREG = SREG, t;
 	
@@ -145,19 +177,17 @@ void ArduinoScheduler::delay(uint32_t ms)
 		if (((uint16_t)micros() - start) >= 1000) {
 			ms--;
 			start += 1000;
+            if (_delay_cb) {
+                _delay_cb();
+            }
 		}
 	}
 }
 
-/* Delay for the given number of microseconds.  Assumes a 8 or 16 MHz clock. */
+/* Delay for the given number of microseconds.  Assumes a 16 MHz clock. */
 void ArduinoScheduler::delay_microseconds(uint16_t us)
 {
-	// calling avrlib's delay_us() function with low values (e.g. 1 or
-	// 2 microseconds) gives delays longer than desired.
-	//delay_us(us);
-
 	// for the 16 MHz clock on most Arduino boards
-
 	// for a one-microsecond delay, simply return.  the overhead
 	// of the function call yields a delay of approximately 1 1/8 us.
 	if (--us == 0)
@@ -178,6 +208,90 @@ void ArduinoScheduler::delay_microseconds(uint16_t us)
 	);
 }
 
-void ArduinoScheduler::register_delay_callback(void(*cb)(void)) {
-    /* XXX need to implement */
+void ArduinoScheduler::register_delay_callback(AP_HAL::Proc proc) {
+    _delay_cb = proc;
+}
+
+void ArduinoScheduler::register_timer_process(
+        AP_HAL::TimedProc proc, uint32_t period_us, uint16_t phase) {
+    /* XXX Assert period_us == 1000 */
+    /* XXX phase meaningless */
+    for (int i = 0; i < _num_timer_procs; i++) {
+        if (_timer_proc[i] == proc) {
+            return;
+        }
+    }
+
+    if (_num_timer_procs < AVR_SCHEDULER_MAX_TIMER_PROCS) {
+        /* this write to _timer_proc can be outside the critical section
+         * because that memory won't be used until _num_timer_procs is
+         * incremented. */
+        _timer_proc[_num_timer_procs] = proc;
+        /* _num_timer_procs is used from interrupt, and multiple bytes long. */
+        cli();
+        _num_timer_procs++;
+        sei();
+    }
+
+}
+
+void ArduinoScheduler::register_timer_failsafe(
+        AP_HAL::TimedProc failsafe, uint32_t period_us) {
+    /* XXX Assert period_us == 1000 */
+    _failsafe = failsafe;
+}
+
+void ArduinoScheduler::suspend_timer_procs() {
+    _timer_suspended = true;
+}
+
+void ArduinoScheduler::resume_timer_procs() {
+    _timer_suspended = false;
+}
+
+void ArduinoScheduler::_timer_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
+    // timer calls taking too long using _in_timer_call.
+    // This approach also gives us a nice uniform spacing between
+    // timer calls
+
+    TCNT2 = RESET_TCNT2_VALUE;
+    sei();
+
+    uint32_t tnow = _micros();
+    if (_in_timer_proc) {
+        // the timer calls took longer than the period of the
+        // timer. This is bad, and may indicate a serious
+        // driver failure. We can't just call the drivers
+        // again, as we could run out of stack. So we only
+        // call the _failsafe call. It's job is to detect if
+        // the drivers or the main loop are indeed dead and to
+        // activate whatever failsafe it thinks may help if
+        // need be.  We assume the failsafe code can't
+        // block. If it does then we will recurse and die when
+        // we run out of stack
+        if (_failsafe != NULL) {
+            _failsafe(tnow);
+        }
+        return;
+    }
+    _in_timer_proc = true;
+
+    if (!_timer_suspended) {
+        // now call the timer based drivers
+        for (int i = 0; i < _num_timer_procs; i++) {
+            if (_timer_proc[i] != NULL) {
+                _timer_proc[i](tnow);
+            }
+        }
+    }
+
+    // and the failsafe, if one is setup
+    if (_failsafe != NULL) {
+        _failsafe(tnow);
+    }
+
+    _in_timer_proc = false;
 }

libraries/AP_HAL_AVR/Scheduler.h

@@ -5,17 +5,46 @@
 #include <AP_HAL.h>
 #include "AP_HAL_AVR_Namespace.h"
 
+#define AVR_SCHEDULER_MAX_TIMER_PROCS 4
+
 class AP_HAL_AVR::ArduinoScheduler : public AP_HAL::Scheduler {
 public:
-    ArduinoScheduler() {}
+    ArduinoScheduler();
     /* AP_HAL::Scheduler methods */
-    void     init();
+
+    /* init: implementation-specific void* argument expected to be an
+     * AP_HAL_AVR::ISRRegistry*. */
+    void     init(void *isrregistry);
     void     delay(uint32_t ms);
     uint32_t millis();
     uint32_t micros();
     void     delay_microseconds(uint16_t us);
-    void     register_delay_callback(void(*cb)(void));
-};
+    void     register_delay_callback(AP_HAL::Proc);
+    void     register_timer_process(AP_HAL::TimedProc,
+                uint32_t period_us, uint16_t phase);
+    void     register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us);
+    void     suspend_timer_procs();
+    void     resume_timer_procs();
 
+private:
+    /* Implementation specific methods: */
+    /* 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();
+
+    /* _micros() is the implementation of micros() as a static private method
+     * so we can use it from inside _timer_event() without virtual dispatch. */
+    static uint32_t _micros();
+
+    AP_HAL::Proc _delay_cb;
+    static AP_HAL::TimedProc _failsafe;
+
+    static volatile bool _timer_suspended;
+    static AP_HAL::TimedProc _timer_proc[AVR_SCHEDULER_MAX_TIMER_PROCS];
+    static uint8_t _num_timer_procs;
+    static bool    _in_timer_proc;
+
+};
 #endif // __AP_HAL_AVR_SCHEDULER_H__