From ba7a596ec30193e6d0f5ca2550314b6a5310844c Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Fri, 7 Sep 2012 16:07:38 -0700 Subject: [PATCH] AP_HAL_AVR: Scheduler extensions implemented --- libraries/AP_HAL_AVR/HAL_AVR.cpp | 2 +- libraries/AP_HAL_AVR/Scheduler.cpp | 140 ++++++++++++++++++++++++++--- libraries/AP_HAL_AVR/Scheduler.h | 37 +++++++- 3 files changed, 161 insertions(+), 18 deletions(-) diff --git a/libraries/AP_HAL_AVR/HAL_AVR.cpp b/libraries/AP_HAL_AVR/HAL_AVR.cpp index 6c9771cbcc..08f6b94332 100644 --- a/libraries/AP_HAL_AVR/HAL_AVR.cpp +++ b/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 */ diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp index 9fb55cd2fe..dfef41700a 100644 --- a/libraries/AP_HAL_AVR/Scheduler.cpp +++ b/libraries/AP_HAL_AVR/Scheduler.cpp @@ -6,7 +6,6 @@ using namespace AP_HAL_AVR; #include #include - #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; } diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h index cada9002b6..5efd01c6b2 100644 --- a/libraries/AP_HAL_AVR/Scheduler.h +++ b/libraries/AP_HAL_AVR/Scheduler.h @@ -5,17 +5,46 @@ #include #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__