From f6038f36bfd080647cd13d7ddf4e16a7de74b88a Mon Sep 17 00:00:00 2001 From: John Arne Birkeland Date: Wed, 30 Jan 2013 20:00:36 +0100 Subject: [PATCH] HAL_AVR: Improved AVRTimer micros() and millis() - More efficient code by using 16-bit timer - micros() now has proper 1 us resolution and less overhead - millis() has less overhead - removed unneeded/unwanted initializatin of timers in AVRTimer::init() --- libraries/AP_HAL_AVR/RCInput_APM1.cpp | 25 +++- libraries/AP_HAL_AVR/RCInput_APM2.cpp | 25 +++- libraries/AP_HAL_AVR/Scheduler.cpp | 6 +- libraries/AP_HAL_AVR/Scheduler_Timer.cpp | 183 +++++++++++------------ 4 files changed, 130 insertions(+), 109 deletions(-) diff --git a/libraries/AP_HAL_AVR/RCInput_APM1.cpp b/libraries/AP_HAL_AVR/RCInput_APM1.cpp index ebdc163917..f5cf6092ff 100644 --- a/libraries/AP_HAL_AVR/RCInput_APM1.cpp +++ b/libraries/AP_HAL_AVR/RCInput_APM1.cpp @@ -65,16 +65,27 @@ void APM1RCInput::init(void* _isrregistry) { * OCR4A: 40000, 0.5us tick => 2ms period / 50hz freq for outbound * fast PWM. */ - TCCR4A = _BV(WGM40) | _BV(WGM41); - TCCR4B = _BV(WGM43) | _BV(WGM42) | _BV(CS41) | _BV(ICES4); - OCR4A = 40000; - /* OCR4B and OCR4C will be used by RCOutput_APM1. init to nil output */ + uint8_t oldSREG = SREG; + cli(); + + /* Timer cleanup before configuring */ + TCNT4 = 0; + TIFR4 = 0; + + /* Set timer 8x prescaler fast PWM mode toggle compare at OCRA with rising edge input capture */ + TCCR4A = _BV(WGM40) | _BV(WGM41); + TCCR4B |= _BV(WGM43) | _BV(WGM42) | _BV(CS41) | _BV(ICES4); + OCR4A = 40000 - 1; // -1 to correct for wrap + + /* OCR4B and OCR4C will be used by RCOutput_APM1. Init to 0xFFFF to prevent premature PWM output */ OCR4B = 0xFFFF; OCR4C = 0xFFFF; /* Enable input capture interrupt */ TIMSK4 |= _BV(ICIE4); + + SREG = oldSREG; } uint8_t APM1RCInput::valid_channels() { return _valid_channels; } @@ -91,9 +102,10 @@ uint16_t APM1RCInput::read(uint8_t ch) { /* constrain ch */ if (ch >= AVR_RC_INPUT_NUM_CHANNELS) return 0; /* grab channel from isr's memory in critical section*/ + uint8_t oldSREG = SREG; cli(); uint16_t capt = _pulse_capt[ch]; - sei(); + SREG = oldSREG; _valid_channels = 0; /* scale _pulse_capt from 0.5us units to 1us units. */ uint16_t pulse = constrain_pulse(capt >> 1); @@ -106,11 +118,12 @@ uint8_t APM1RCInput::read(uint16_t* periods, uint8_t len) { /* constrain len */ if (len > AVR_RC_INPUT_NUM_CHANNELS) { len = AVR_RC_INPUT_NUM_CHANNELS; } /* grab channels from isr's memory in critical section */ + uint8_t oldSREG = SREG; cli(); for (uint8_t i = 0; i < len; i++) { periods[i] = _pulse_capt[i]; } - sei(); + SREG = oldSREG; /* Outside of critical section, do the math (in place) to scale and * constrain the pulse. */ for (uint8_t i = 0; i < len; i++) { diff --git a/libraries/AP_HAL_AVR/RCInput_APM2.cpp b/libraries/AP_HAL_AVR/RCInput_APM2.cpp index 31c4f2d46f..123ca6c948 100644 --- a/libraries/AP_HAL_AVR/RCInput_APM2.cpp +++ b/libraries/AP_HAL_AVR/RCInput_APM2.cpp @@ -65,16 +65,27 @@ void APM2RCInput::init(void* _isrregistry) { * OCR5A: 40000, 0.5us tick => 2ms period / 50hz freq for outbound * fast PWM. */ - TCCR5A = _BV(WGM50) | _BV(WGM51); - TCCR5B = _BV(WGM53) | _BV(WGM52) | _BV(CS51) | _BV(ICES5); - OCR5A = 40000; - /* OCR5B and OCR5C will be used by RCOutput_APM2. init to nil output */ + uint8_t oldSREG = SREG; + cli(); + + /* Timer cleanup before configuring */ + TCNT5 = 0; + TIFR5 = 0; + + /* Set timer 8x prescaler fast PWM mode toggle compare at OCRA with rising edge input capture */ + TCCR5A = _BV(WGM50) | _BV(WGM51); + TCCR5B |= _BV(WGM53) | _BV(WGM52) | _BV(CS51) | _BV(ICES5); + OCR5A = 40000 - 1; // -1 to correct for wrap + + /* OCR5B and OCR5C will be used by RCOutput_APM2. Init to 0xFFFF to prevent premature PWM output */ OCR5B = 0xFFFF; OCR5C = 0xFFFF; /* Enable input capture interrupt */ TIMSK5 |= _BV(ICIE5); + + SREG = oldSREG; } uint8_t APM2RCInput::valid_channels() { return _valid_channels; } @@ -91,9 +102,10 @@ uint16_t APM2RCInput::read(uint8_t ch) { /* constrain ch */ if (ch >= AVR_RC_INPUT_NUM_CHANNELS) return 0; /* grab channel from isr's memory in critical section*/ + uint8_t oldSREG = SREG; cli(); uint16_t capt = _pulse_capt[ch]; - sei(); + SREG = oldSREG; _valid_channels = 0; /* scale _pulse_capt from 0.5us units to 1us units. */ uint16_t pulse = constrain_pulse(capt >> 1); @@ -106,11 +118,12 @@ uint8_t APM2RCInput::read(uint16_t* periods, uint8_t len) { /* constrain len */ if (len > AVR_RC_INPUT_NUM_CHANNELS) { len = AVR_RC_INPUT_NUM_CHANNELS; } /* grab channels from isr's memory in critical section */ + uint8_t oldSREG = SREG; cli(); for (int i = 0; i < len; i++) { periods[i] = _pulse_capt[i]; } - sei(); + SREG = oldSREG; /* Outside of critical section, do the math (in place) to scale and * constrain the pulse. */ for (int i = 0; i < len; i++) { diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp index be9db4a706..30ac05da69 100644 --- a/libraries/AP_HAL_AVR/Scheduler.cpp +++ b/libraries/AP_HAL_AVR/Scheduler.cpp @@ -37,8 +37,7 @@ AVRScheduler::AVRScheduler() : void AVRScheduler::init(void* _isrregistry) { ISRRegistry* isrregistry = (ISRRegistry*) _isrregistry; - /* _timer: sets up timer hardware to Arduino defaults, and - * uses TIMER0 to implement millis & micros */ + /* _timer: sets up timer hardware to implement millis & micros. */ _timer.init(); /* TIMER2: Setup the overflow interrupt to occur at 1khz. */ @@ -50,6 +49,9 @@ void AVRScheduler::init(void* _isrregistry) { TIMSK2 = _BV(TOIE2); /* Enable overflow interrupt*/ /* Register _timer_isr_event to trigger on overflow */ isrregistry->register_signal(ISR_REGISTRY_TIMER2_OVF, _timer_isr_event); + + /* Turn on global interrupt flag, AVR interupt system will start from this point */ + sei(); } uint32_t AVRScheduler::micros() { diff --git a/libraries/AP_HAL_AVR/Scheduler_Timer.cpp b/libraries/AP_HAL_AVR/Scheduler_Timer.cpp index 01d186ee5f..d73d450c2e 100644 --- a/libraries/AP_HAL_AVR/Scheduler_Timer.cpp +++ b/libraries/AP_HAL_AVR/Scheduler_Timer.cpp @@ -10,47 +10,57 @@ 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; +#if (CONFIG_HAL_BOARD == HAL_BOARD_APM1 ) +#define AVR_TIMER_OVF_VECT TIMER4_OVF_vect +#define AVR_TIMER_TCNT TCNT4 +#define AVR_TIMER_TIFR TIFR4 +#define AVR_TIMER_TCCRA TCCR4A +#define AVR_TIMER_TCCRB TCCR4B +#define AVR_TIMER_OCRA OCR4A +#define AVR_TIMER_TIMSK TIMSK4 +#define AVR_TIMER_TOIE TOIE4 +#define AVR_TIMER_WGM0 WGM40 +#define AVR_TIMER_WGM1 WGM41 +#define AVR_TIMER_WGM2 WGM42 +#define AVR_TIMER_WGM3 WGM43 +#define AVR_TIMER_CS1 CS41 +#elif (CONFIG_HAL_BOARD == HAL_BOARD_APM2 ) +#define AVR_TIMER_OVF_VECT TIMER5_OVF_vect +#define AVR_TIMER_TCNT TCNT5 +#define AVR_TIMER_TIFR TIFR5 +#define AVR_TIMER_TCCRA TCCR5A +#define AVR_TIMER_TCCRB TCCR5B +#define AVR_TIMER_OCRA OCR5A +#define AVR_TIMER_TIMSK TIMSK5 +#define AVR_TIMER_TOIE TOIE5 +#define AVR_TIMER_WGM0 WGM50 +#define AVR_TIMER_WGM1 WGM51 +#define AVR_TIMER_WGM2 WGM52 +#define AVR_TIMER_WGM3 WGM53 +#define AVR_TIMER_CS1 CS51 + +#endif + +static volatile uint32_t timer_micros_counter = 0; +static volatile uint32_t timer_millis_counter = 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); + uint8_t oldSREG = SREG; + cli(); - // 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 + // Timer cleanup before configuring + AVR_TIMER_TCNT = 0; + AVR_TIMER_TIFR = 0; + + // Set timer 8x prescaler fast PWM mode toggle compare at OCRA + AVR_TIMER_TCCRA = _BV( AVR_TIMER_WGM0 ) | _BV( AVR_TIMER_WGM1 ); + AVR_TIMER_TCCRB |= _BV( AVR_TIMER_WGM3 ) | _BV( AVR_TIMER_WGM2 ) | _BV( AVR_TIMER_CS1 ); + AVR_TIMER_OCRA = 40000 - 1; // -1 to correct for wrap - 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 + // Enable overflow interrupt + AVR_TIMER_TIMSK |= _BV( AVR_TIMER_TOIE ); // set a2d prescale factor to 128 // 16 MHz / 128 = 125 KHz, inside the desired 50-200 KHz range. @@ -67,76 +77,59 @@ void AVRTimer::init() { // here so they can be used as normal digital i/o; they will be // reconnected in Serial.begin() UCSR0B = 0; + + SREG = oldSREG; } -#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) +SIGNAL( AVR_TIMER_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; + // Hardcoded for AVR@16MHZ and 8x pre-scale 16-bit timer overflow at 40000 + timer_micros_counter += 40000 / 2; // 20000us each overflow + timer_millis_counter += 40000 / 2000; // 20ms each overlflow } uint32_t AVRTimer::micros() { - uint32_t m; - uint8_t t; - - uint8_t oldSREG = SREG; + uint8_t oldSREG = SREG; cli(); - m = timer0_overflow_count; - t = TCNT0; - - if ((TIFR0 & _BV(TOV0)) && (t < 255)) - m++; - - SREG = oldSREG; + // Hardcoded for AVR@16MHZ and 8x pre-scale 16-bit timer + //uint32_t time_micros = timer_micros_counter + (AVR_TIMER_TCNT / 2); + //uint32_t time_micros = timer_micros_counter + (AVR_TIMER_TCNT >> 1); + + uint32_t time_micros = timer_micros_counter; + uint16_t tcnt = AVR_TIMER_TCNT; - return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond()); + // Check for imminent timer overflow interrupt and pre-increment counter + if ( AVR_TIMER_TIFR & 1 && tcnt < 39999 ) + { + time_micros += 40000 / 2; + } + SREG = oldSREG; + + return time_micros + (tcnt >> 1); } +uint32_t AVRTimer::millis() { + uint8_t oldSREG = SREG; + cli(); + // Hardcoded for AVR@16MHZ and 8x pre-scale 16-bit timer + //uint32_t time_millis = timer_millis_counter + (AVR_TIMER_TCNT / 2000) ; + //uint32_t time_millis = timer_millis_counter + (AVR_TIMER_TCNT >> 11); // AVR_TIMER_CNT / 2048 is close enough (24us counter delay) + + uint32_t time_millis = timer_millis_counter; + uint16_t tcnt = AVR_TIMER_TCNT; + + // Check for imminent timer overflow interrupt and pre-increment counter + if ( AVR_TIMER_TIFR & 1 && tcnt < 39999 ) + { + time_millis += 40000 / 2000; + } + SREG = oldSREG; + + return time_millis + (tcnt >> 11); +} + + /* Delay for the given number of microseconds. Assumes a 16 MHz clock. */ void AVRTimer::delay_microseconds(uint16_t us) {