aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_AVR: Improved AVRTimer micros() and millis()

Browse Source 
- 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()
This commit is contained in:
John Arne Birkeland 2013-01-30 20:00:36 +01:00 committed by Randy Mackay
parent 5ba34b38c1
commit f6038f36bf
4 changed files with 130 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_HAL_AVR/RCInput_APM1.cpp
View File

@ -65,16 +65,27 @@ void APM1RCInput::init(void* _isrregistry) {
* OCR4A: 40000, 0.5us tick => 2ms period / 50hz freq for outbound * OCR4A: 40000, 0.5us tick => 2ms period / 50hz freq for outbound
* fast PWM. * 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; OCR4B = 0xFFFF;
OCR4C = 0xFFFF; OCR4C = 0xFFFF;
/* Enable input capture interrupt */ /* Enable input capture interrupt */
TIMSK4 |= _BV(ICIE4); TIMSK4 |= _BV(ICIE4);
SREG = oldSREG;
} }
uint8_t APM1RCInput::valid_channels() { return _valid_channels; } uint8_t APM1RCInput::valid_channels() { return _valid_channels; }
@ -91,9 +102,10 @@ uint16_t APM1RCInput::read(uint8_t ch) {
/* constrain ch */ /* constrain ch */
if (ch >= AVR_RC_INPUT_NUM_CHANNELS) return 0; if (ch >= AVR_RC_INPUT_NUM_CHANNELS) return 0;
/* grab channel from isr's memory in critical section*/ /* grab channel from isr's memory in critical section*/
uint8_t oldSREG = SREG;
cli(); cli();
uint16_t capt = _pulse_capt[ch]; uint16_t capt = _pulse_capt[ch];
sei(); SREG = oldSREG;
_valid_channels = 0; _valid_channels = 0;
/* scale _pulse_capt from 0.5us units to 1us units. */ /* scale _pulse_capt from 0.5us units to 1us units. */
uint16_t pulse = constrain_pulse(capt >> 1); uint16_t pulse = constrain_pulse(capt >> 1);
@ -106,11 +118,12 @@ uint8_t APM1RCInput::read(uint16_t* periods, uint8_t len) {
/* constrain len */ /* constrain len */
if (len > AVR_RC_INPUT_NUM_CHANNELS) { len = AVR_RC_INPUT_NUM_CHANNELS; } if (len > AVR_RC_INPUT_NUM_CHANNELS) { len = AVR_RC_INPUT_NUM_CHANNELS; }
/* grab channels from isr's memory in critical section */ /* grab channels from isr's memory in critical section */
uint8_t oldSREG = SREG;
cli(); cli();
for (uint8_t i = 0; i < len; i++) { for (uint8_t i = 0; i < len; i++) {
periods[i] = _pulse_capt[i]; periods[i] = _pulse_capt[i];
} }
sei(); SREG = oldSREG;
/* Outside of critical section, do the math (in place) to scale and /* Outside of critical section, do the math (in place) to scale and
* constrain the pulse. */ * constrain the pulse. */
for (uint8_t i = 0; i < len; i++) { for (uint8_t i = 0; i < len; i++) {

25
libraries/AP_HAL_AVR/RCInput_APM2.cpp
View File

@ -65,16 +65,27 @@ void APM2RCInput::init(void* _isrregistry) {
* OCR5A: 40000, 0.5us tick => 2ms period / 50hz freq for outbound * OCR5A: 40000, 0.5us tick => 2ms period / 50hz freq for outbound
* fast PWM. * 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; OCR5B = 0xFFFF;
OCR5C = 0xFFFF; OCR5C = 0xFFFF;
/* Enable input capture interrupt */ /* Enable input capture interrupt */
TIMSK5 |= _BV(ICIE5); TIMSK5 |= _BV(ICIE5);
SREG = oldSREG;
} }
uint8_t APM2RCInput::valid_channels() { return _valid_channels; } uint8_t APM2RCInput::valid_channels() { return _valid_channels; }
@ -91,9 +102,10 @@ uint16_t APM2RCInput::read(uint8_t ch) {
/* constrain ch */ /* constrain ch */
if (ch >= AVR_RC_INPUT_NUM_CHANNELS) return 0; if (ch >= AVR_RC_INPUT_NUM_CHANNELS) return 0;
/* grab channel from isr's memory in critical section*/ /* grab channel from isr's memory in critical section*/
uint8_t oldSREG = SREG;
cli(); cli();
uint16_t capt = _pulse_capt[ch]; uint16_t capt = _pulse_capt[ch];
sei(); SREG = oldSREG;
_valid_channels = 0; _valid_channels = 0;
/* scale _pulse_capt from 0.5us units to 1us units. */ /* scale _pulse_capt from 0.5us units to 1us units. */
uint16_t pulse = constrain_pulse(capt >> 1); uint16_t pulse = constrain_pulse(capt >> 1);
@ -106,11 +118,12 @@ uint8_t APM2RCInput::read(uint16_t* periods, uint8_t len) {
/* constrain len */ /* constrain len */
if (len > AVR_RC_INPUT_NUM_CHANNELS) { len = AVR_RC_INPUT_NUM_CHANNELS; } if (len > AVR_RC_INPUT_NUM_CHANNELS) { len = AVR_RC_INPUT_NUM_CHANNELS; }
/* grab channels from isr's memory in critical section */ /* grab channels from isr's memory in critical section */
uint8_t oldSREG = SREG;
cli(); cli();
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++) {
periods[i] = _pulse_capt[i]; periods[i] = _pulse_capt[i];
} }
sei(); SREG = oldSREG;
/* Outside of critical section, do the math (in place) to scale and /* Outside of critical section, do the math (in place) to scale and
* constrain the pulse. */ * constrain the pulse. */
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++) {

6
libraries/AP_HAL_AVR/Scheduler.cpp
View File

@ -37,8 +37,7 @@ AVRScheduler::AVRScheduler() :
void AVRScheduler::init(void* _isrregistry) { void AVRScheduler::init(void* _isrregistry) {
ISRRegistry* isrregistry = (ISRRegistry*) _isrregistry; ISRRegistry* isrregistry = (ISRRegistry*) _isrregistry;
/* _timer: sets up timer hardware to Arduino defaults, and /* _timer: sets up timer hardware to implement millis & micros. */
* uses TIMER0 to implement millis & micros */
_timer.init(); _timer.init();
/* TIMER2: Setup the overflow interrupt to occur at 1khz. */ /* TIMER2: Setup the overflow interrupt to occur at 1khz. */
@ -50,6 +49,9 @@ void AVRScheduler::init(void* _isrregistry) {
TIMSK2 = _BV(TOIE2); /* Enable overflow interrupt*/ TIMSK2 = _BV(TOIE2); /* Enable overflow interrupt*/
/* Register _timer_isr_event to trigger on overflow */ /* Register _timer_isr_event to trigger on overflow */
isrregistry->register_signal(ISR_REGISTRY_TIMER2_OVF, _timer_isr_event); 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() { uint32_t AVRScheduler::micros() {

183
libraries/AP_HAL_AVR/Scheduler_Timer.cpp
View File

@ -10,47 +10,57 @@ using namespace AP_HAL_AVR;
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
static volatile uint32_t timer0_overflow_count = 0; #if (CONFIG_HAL_BOARD == HAL_BOARD_APM1 )
static volatile uint32_t timer0_millis = 0; #define AVR_TIMER_OVF_VECT TIMER4_OVF_vect
static uint8_t timer0_fract = 0; #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() { void AVRTimer::init() {
// this needs to be called before setup() or some functions won't uint8_t oldSREG = SREG;
// work there cli();
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 // Timer cleanup before configuring
// this is better for motors as it ensures an even waveform AVR_TIMER_TCNT = 0;
// note, however, that fast pwm mode can achieve a frequency of up AVR_TIMER_TIFR = 0;
// 8 MHz (with a 16 MHz clock) at 50% duty cycle
// 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; // Enable overflow interrupt
AVR_TIMER_TIMSK |= _BV( AVR_TIMER_TOIE );
// 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 // set a2d prescale factor to 128
// 16 MHz / 128 = 125 KHz, inside the desired 50-200 KHz range. // 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 // here so they can be used as normal digital i/o; they will be
// reconnected in Serial.begin() // reconnected in Serial.begin()
UCSR0B = 0; UCSR0B = 0;
SREG = oldSREG;
} }
#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L ) SIGNAL( AVR_TIMER_OVF_VECT)
#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 // Hardcoded for AVR@16MHZ and 8x pre-scale 16-bit timer overflow at 40000
// (volatile variables must be read from memory on every access) timer_micros_counter += 40000 / 2; // 20000us each overflow
uint32_t m = timer0_millis; timer_millis_counter += 40000 / 2000; // 20ms each overlflow
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 AVRTimer::micros() {
uint32_t m; uint8_t oldSREG = SREG;
uint8_t t;
uint8_t oldSREG = SREG;
cli(); cli();
m = timer0_overflow_count; // Hardcoded for AVR@16MHZ and 8x pre-scale 16-bit timer
t = TCNT0; //uint32_t time_micros = timer_micros_counter + (AVR_TIMER_TCNT / 2);
//uint32_t time_micros = timer_micros_counter + (AVR_TIMER_TCNT >> 1);
if ((TIFR0 & _BV(TOV0)) && (t < 255))
m++; uint32_t time_micros = timer_micros_counter;
uint16_t tcnt = AVR_TIMER_TCNT;
SREG = oldSREG;
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. */ /* Delay for the given number of microseconds. Assumes a 16 MHz clock. */
void AVRTimer::delay_microseconds(uint16_t us) void AVRTimer::delay_microseconds(uint16_t us)
{ {