ardupilot/libraries/AP_HAL_AVR/Scheduler_Timer.cpp

158 lines
4.8 KiB
C++
Raw Normal View History

#include <AP_HAL/AP_HAL.h>
#if (CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2)
#include <avr/io.h>
#include <avr/interrupt.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))
#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() {
uint8_t oldSREG = SREG;
cli();
// 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
// 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.
// 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;
SREG = oldSREG;
}
SIGNAL( AVR_TIMER_OVF_VECT)
{
// 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() {
uint8_t oldSREG = SREG;
cli();
// 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;
// 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)
{
// 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
);
}
#endif