From f9e9b8a7efd3a002da3505a5fccaaa9a7d89530a Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Thu, 6 Dec 2012 15:24:55 -0800 Subject: [PATCH] AP_HAL_AVR: move scheduler's timer-hw dependent methods to a separate cpp --- libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h | 1 + libraries/AP_HAL_AVR/Scheduler.cpp | 172 ++------------------ libraries/AP_HAL_AVR/Scheduler.h | 17 +- libraries/AP_HAL_AVR/Scheduler_Timer.cpp | 161 ++++++++++++++++++ 4 files changed, 189 insertions(+), 162 deletions(-) create mode 100644 libraries/AP_HAL_AVR/Scheduler_Timer.cpp diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h index bbb7db9636..9820f847d9 100644 --- a/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h +++ b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h @@ -26,6 +26,7 @@ namespace AP_HAL_AVR { class APM1RCOutput; class APM2RCOutput; class AVRScheduler; + class AVRTimer; class AVRSemaphore; class ISRRegistry; } diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp index b9f446d7a4..c13f815894 100644 --- a/libraries/AP_HAL_AVR/Scheduler.cpp +++ b/libraries/AP_HAL_AVR/Scheduler.cpp @@ -1,4 +1,3 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include #include @@ -7,20 +6,15 @@ #include "Scheduler.h" using namespace AP_HAL_AVR; -#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) -#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) - extern const AP_HAL::HAL& hal; -static volatile uint32_t timer0_overflow_count = 0; -static volatile uint32_t timer0_millis = 0; -static uint8_t timer0_fract = 0; - /* AVRScheduler timer interrupt period is controlled by TCNT2. * 256-62 gives a 1kHz period. */ #define RESET_TCNT2_VALUE (256 - 62) /* Static AVRScheduler variables: */ +AVRTimer AVRScheduler::_timer; + AP_HAL::TimedProc AVRScheduler::_failsafe = NULL; volatile bool AVRScheduler::_timer_suspended = false; AP_HAL::TimedProc AVRScheduler::_timer_proc[AVR_SCHEDULER_MAX_TIMER_PROCS] = {NULL}; @@ -38,57 +32,9 @@ AVRScheduler::AVRScheduler() : void AVRScheduler::init(void* _isrregistry) { ISRRegistry* isrregistry = (ISRRegistry*) _isrregistry; - // this needs to be called before setup() or some functions won't - // work there - sei(); - - // set timer 0 prescale factor to 64 - // this combination is for the standard 168/328/1280/2560 - sbi(TCCR0B, CS01); - sbi(TCCR0B, CS00); - // enable timer 0 overflow interrupt - sbi(TIMSK0, TOIE0); - - // timers 1 and 2 are used for phase-correct hardware pwm - // this is better for motors as it ensures an even waveform - // note, however, that fast pwm mode can achieve a frequency of up - // 8 MHz (with a 16 MHz clock) at 50% duty cycle - - TCCR1B = 0; - - // set timer 1 prescale factor to 64 - sbi(TCCR1B, CS11); - sbi(TCCR1B, CS10); - // put timer 1 in 8-bit phase correct pwm mode - sbi(TCCR1A, WGM10); - - 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 - - sbi(TCCR4B, CS41); // set timer 4 prescale factor to 64 - sbi(TCCR4B, CS40); - sbi(TCCR4A, WGM40); // put timer 4 in 8-bit phase correct pwm mode - - sbi(TCCR5B, CS51); // set timer 5 prescale factor to 64 - sbi(TCCR5B, CS50); - sbi(TCCR5A, WGM50); // put timer 5 in 8-bit phase correct pwm mode - - // set a2d prescale factor to 128 - // 16 MHz / 128 = 125 KHz, inside the desired 50-200 KHz range. - // XXX: this will not work properly for other clock speeds, and - // this code should use F_CPU to determine the prescale factor. - sbi(ADCSRA, ADPS2); - sbi(ADCSRA, ADPS1); - sbi(ADCSRA, ADPS0); - - // enable a2d conversions - sbi(ADCSRA, ADEN); - - // the bootloader connects pins 0 and 1 to the USART; disconnect them - // here so they can be used as normal digital i/o; they will be - // reconnected in Serial.begin() - UCSR0B = 0; + /* _timer: sets up timer hardware to Arduino defaults, and + * uses TIMER0 to implement millis & micros */ + _timer.init(); /* TIMER2: Setup the overflow interrupt to occur at 1khz. */ TIMSK2 = 0; /* Disable timer interrupt */ @@ -101,88 +47,24 @@ void AVRScheduler::init(void* _isrregistry) { isrregistry->register_signal(ISR_REGISTRY_TIMER2_OVF, _timer_event); } -#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L ) -#define clockCyclesToMicroseconds(a) ( ((a) * 1000L) / (F_CPU / 1000L) ) - -// the prescaler is set so that timer0 ticks every 64 clock cycles, and the -// the overflow handler is called every 256 ticks. -#define MICROSECONDS_PER_TIMER0_OVERFLOW (clockCyclesToMicroseconds(64 * 256)) - -// the whole number of milliseconds per timer0 overflow -#define MILLIS_INC (MICROSECONDS_PER_TIMER0_OVERFLOW / 1000) - -// the fractional number of milliseconds per timer0 overflow. we shift right -// by three to fit these numbers into a byte. (for the clock speeds we care -// about - 8 and 16 MHz - this doesn't lose precision.) -#define FRACT_INC ((MICROSECONDS_PER_TIMER0_OVERFLOW % 1000) >> 3) -#define FRACT_MAX (1000 >> 3) - - -SIGNAL(TIMER0_OVF_vect) -{ - // copy these to local variables so they can be stored in registers - // (volatile variables must be read from memory on every access) - uint32_t m = timer0_millis; - uint8_t f = timer0_fract; - - m += MILLIS_INC; - f += FRACT_INC; - if (f >= FRACT_MAX) { - f -= FRACT_MAX; - m += 1; - } - - timer0_fract = f; - timer0_millis = m; - timer0_overflow_count++; -} - -uint32_t AVRScheduler::millis() -{ - uint32_t m; - uint8_t oldSREG = SREG; - - // disable interrupts while we read timer0_millis or we might get an - // inconsistent value (e.g. in the middle of a write to timer0_millis) - cli(); - m = timer0_millis; - SREG = oldSREG; - - return m; -} - -/* micros() is essentially a static method, but we need it to be available - * via virtual dispatch through the hal. */ uint32_t AVRScheduler::micros() { - return _micros(); + return _timer.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 AVRScheduler::_micros() { - uint32_t m; - uint8_t t; - - uint8_t oldSREG = SREG; - cli(); +uint32_t AVRScheduler::millis() { + return _timer.millis(); +} - m = timer0_overflow_count; - t = TCNT0; - - if ((TIFR0 & _BV(TOV0)) && (t < 255)) - m++; - - SREG = oldSREG; - - return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond()); +void AVRScheduler::delay_microseconds(uint16_t us) { + _timer.delay_microseconds(us); } void AVRScheduler::delay(uint32_t ms) { - uint32_t start = _micros(); + uint32_t start = _timer.micros(); while (ms > 0) { - while ((micros() - start) >= 1000) { + while ((_timer.micros() - start) >= 1000) { ms--; if (ms == 0) break; start += 1000; @@ -195,30 +77,6 @@ void AVRScheduler::delay(uint32_t ms) } } -/* Delay for the given number of microseconds. Assumes a 16 MHz clock. */ -void AVRScheduler::delay_microseconds(uint16_t 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) - return; - - // the following loop takes a quarter of a microsecond (4 cycles) - // per iteration, so execute it four times for each microsecond of - // delay requested. - us <<= 2; - - // account for the time taken in the preceeding commands. - us -= 2; - - // busy wait - __asm__ __volatile__ ( - "1: sbiw %0,1" "\n\t" // 2 cycles - "brne 1b" : "=w" (us) : "0" (us) // 2 cycles - ); -} - void AVRScheduler::register_delay_callback(AP_HAL::Proc proc, uint16_t min_time_ms) { _delay_cb = proc; @@ -252,7 +110,7 @@ bool AVRScheduler::defer_timer_process(AP_HAL::TimedProc proc) { } else { _timer_suspended = true; sei(); - proc(micros()); + proc(_timer.micros()); _timer_suspended = false; return true; } @@ -283,7 +141,7 @@ void AVRScheduler::_timer_event() { TCNT2 = RESET_TCNT2_VALUE; sei(); - uint32_t tnow = _micros(); + uint32_t tnow = _timer.micros(); if (_in_timer_proc) { // the timer calls took longer than the period of the // timer. This is bad, and may indicate a serious diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h index 0a9afef001..a34374d530 100644 --- a/libraries/AP_HAL_AVR/Scheduler.h +++ b/libraries/AP_HAL_AVR/Scheduler.h @@ -7,6 +7,16 @@ #define AVR_SCHEDULER_MAX_TIMER_PROCS 4 +/* Class for managing the AVR Timers: */ +class AP_HAL_AVR::AVRTimer { +public: + static void init(); + static uint32_t millis(); + static uint32_t micros(); + static void delay_microseconds(uint16_t us); +}; + +/* Scheduler implementation: */ class AP_HAL_AVR::AVRScheduler : public AP_HAL::Scheduler { public: AVRScheduler(); @@ -30,16 +40,13 @@ public: void reboot(); private: - /* Implementation specific methods: */ + 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(); - /* _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; uint16_t _min_delay_cb_ms; static AP_HAL::TimedProc _failsafe; diff --git a/libraries/AP_HAL_AVR/Scheduler_Timer.cpp b/libraries/AP_HAL_AVR/Scheduler_Timer.cpp new file mode 100644 index 0000000000..957361697e --- /dev/null +++ b/libraries/AP_HAL_AVR/Scheduler_Timer.cpp @@ -0,0 +1,161 @@ + +#include +#include + +#include "HAL_AVR.h" +#include "Scheduler.h" +using namespace AP_HAL_AVR; + +#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) + +static volatile uint32_t timer0_overflow_count = 0; +static volatile uint32_t timer0_millis = 0; +static uint8_t timer0_fract = 0; + + +void AVRTimer::init() { + // this needs to be called before setup() or some functions won't + // work there + sei(); + + // set timer 0 prescale factor to 64 + // this combination is for the standard 168/328/1280/2560 + sbi(TCCR0B, CS01); + sbi(TCCR0B, CS00); + // enable timer 0 overflow interrupt + sbi(TIMSK0, TOIE0); + + // timers 1 and 2 are used for phase-correct hardware pwm + // this is better for motors as it ensures an even waveform + // note, however, that fast pwm mode can achieve a frequency of up + // 8 MHz (with a 16 MHz clock) at 50% duty cycle + + TCCR1B = 0; + + // set timer 1 prescale factor to 64 + sbi(TCCR1B, CS11); + sbi(TCCR1B, CS10); + // put timer 1 in 8-bit phase correct pwm mode + sbi(TCCR1A, WGM10); + + 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 + + sbi(TCCR4B, CS41); // set timer 4 prescale factor to 64 + sbi(TCCR4B, CS40); + sbi(TCCR4A, WGM40); // put timer 4 in 8-bit phase correct pwm mode + + sbi(TCCR5B, CS51); // set timer 5 prescale factor to 64 + sbi(TCCR5B, CS50); + sbi(TCCR5A, WGM50); // put timer 5 in 8-bit phase correct pwm mode + + // set a2d prescale factor to 128 + // 16 MHz / 128 = 125 KHz, inside the desired 50-200 KHz range. + // XXX: this will not work properly for other clock speeds, and + // this code should use F_CPU to determine the prescale factor. + sbi(ADCSRA, ADPS2); + sbi(ADCSRA, ADPS1); + sbi(ADCSRA, ADPS0); + + // enable a2d conversions + sbi(ADCSRA, ADEN); + + // the bootloader connects pins 0 and 1 to the USART; disconnect them + // here so they can be used as normal digital i/o; they will be + // reconnected in Serial.begin() + UCSR0B = 0; +} + +#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L ) +#define clockCyclesToMicroseconds(a) ( ((a) * 1000L) / (F_CPU / 1000L) ) + +// the prescaler is set so that timer0 ticks every 64 clock cycles, and the +// the overflow handler is called every 256 ticks. +#define MICROSECONDS_PER_TIMER0_OVERFLOW (clockCyclesToMicroseconds(64 * 256)) + +// the whole number of milliseconds per timer0 overflow +#define MILLIS_INC (MICROSECONDS_PER_TIMER0_OVERFLOW / 1000) + +// the fractional number of milliseconds per timer0 overflow. we shift right +// by three to fit these numbers into a byte. (for the clock speeds we care +// about - 8 and 16 MHz - this doesn't lose precision.) +#define FRACT_INC ((MICROSECONDS_PER_TIMER0_OVERFLOW % 1000) >> 3) +#define FRACT_MAX (1000 >> 3) + + +SIGNAL(TIMER0_OVF_vect) +{ + // copy these to local variables so they can be stored in registers + // (volatile variables must be read from memory on every access) + uint32_t m = timer0_millis; + uint8_t f = timer0_fract; + + m += MILLIS_INC; + f += FRACT_INC; + if (f >= FRACT_MAX) { + f -= FRACT_MAX; + m += 1; + } + + timer0_fract = f; + timer0_millis = m; + timer0_overflow_count++; +} + +uint32_t AVRTimer::millis() +{ + uint32_t m; + uint8_t oldSREG = SREG; + + // disable interrupts while we read timer0_millis or we might get an + // inconsistent value (e.g. in the middle of a write to timer0_millis) + cli(); + m = timer0_millis; + SREG = oldSREG; + + return m; +} + +uint32_t AVRTimer::micros() { + uint32_t m; + uint8_t t; + + uint8_t oldSREG = SREG; + cli(); + + m = timer0_overflow_count; + t = TCNT0; + + if ((TIFR0 & _BV(TOV0)) && (t < 255)) + m++; + + SREG = oldSREG; + + return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond()); +} + +/* Delay for the given number of microseconds. Assumes a 16 MHz clock. */ +void AVRTimer::delay_microseconds(uint16_t 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) + return; + + // the following loop takes a quarter of a microsecond (4 cycles) + // per iteration, so execute it four times for each microsecond of + // delay requested. + us <<= 2; + + // account for the time taken in the preceeding commands. + us -= 2; + + // busy wait + __asm__ __volatile__ ( + "1: sbiw %0,1" "\n\t" // 2 cycles + "brne 1b" : "=w" (us) : "0" (us) // 2 cycles + ); +}