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

AP_HAL: Add scheduler interface, move Arduino init code to implementation

Browse Source
This commit is contained in:
Pat Hickey 2012-08-23 11:36:13 -07:00 committed by Andrew Tridgell
parent 59a94d5aac
commit ea8242ace1
10 changed files with 237 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_HAL/AP_HAL.h
View File

@ -15,6 +15,7 @@
#include "GPIO.h"
#include "PPMInput.h"
#include "PWMOutput.h"
#include "Scheduler.h"
#include "utility/Print.h"
#include "utility/Stream.h"

1
libraries/AP_HAL/AP_HAL_Namespace.h
View File

@ -18,6 +18,7 @@ namespace AP_HAL {
class GPIO;
class PPMInput;
class PWMOutput;
class Scheduler;
class EmptyUARTDriver;

7
libraries/AP_HAL/HAL.h
View File

@ -28,7 +28,8 @@ public:
AP_HAL::Console* _console,
AP_HAL::GPIO* _gpio,
AP_HAL::PPMInput* _ppmin,
AP_HAL::PWMOutput* _pwmout)
AP_HAL::PWMOutput* _pwmout,
AP_HAL::Scheduler* _scheduler)
:
uart0(_uart0),
uart1(_uart1),
@ -42,7 +43,8 @@ public:
console(_console),
gpio(_gpio),
ppmin(_ppmin),
pwmout(_pwmout)
pwmout(_pwmout),
scheduler(_scheduler)
{}
virtual void init(void* opts) const = 0;
@ -60,6 +62,7 @@ public:
AP_HAL::GPIO* gpio;
AP_HAL::PPMInput* ppmin;
AP_HAL::PWMOutput* pwmout;
AP_HAL::Scheduler* scheduler;
};
#endif // __AP_HAL_HAL_H__

18
libraries/AP_HAL/Scheduler.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef __AP_HAL_SCHEDULER_H__
#define __AP_HAL_SCHEDULER_H__
#include "AP_HAL_Namespace.h"
class AP_HAL::Scheduler {
public:
Scheduler() {}
virtual void init() = 0;
virtual void delay(unsigned long ms) = 0;
virtual unsigned long millis() = 0;
virtual unsigned long micros() = 0;
virtual void delayMicroseconds(unsigned int us) = 0;
};
#endif // __AP_HAL_SCHEDULER_H__

8
libraries/AP_HAL_AVR/AP_HAL_AVR.cpp
View File

@ -14,6 +14,7 @@
#include "GPIO.h"
#include "PPMInput.h"
#include "PWMOutput.h"
#include "Scheduler.h"
using namespace AP_HAL;
using namespace AP_HAL_AVR;
@ -39,6 +40,7 @@ static APM1PPMInput apm1PPMInput;
static APM2PPMInput apm2PPMInput;
static APM1PWMOutput apm1PWMOutput;
static APM2PWMOutput apm2PWMOutput;
static ArduinoScheduler arduinoScheduler;
const HAL_AVR AP_HAL_AVR_APM1(
(UARTDriver*) &avrUart0Driver,
@ -53,7 +55,8 @@ const HAL_AVR AP_HAL_AVR_APM1(
&avrUartConsole,
&arduinoGPIO,
&apm1PPMInput,
&apm1PWMOutput );
&apm1PWMOutput,
&arduinoScheduler );
const HAL_AVR AP_HAL_AVR_APM2(
(UARTDriver*) &avrUart0Driver,
@ -68,5 +71,6 @@ const HAL_AVR AP_HAL_AVR_APM2(
&avrUartConsole,
&arduinoGPIO,
&apm2PPMInput,
&apm2PWMOutput );
&apm2PWMOutput,
&arduinoScheduler );

1
libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h
View File

@ -18,6 +18,7 @@ namespace AP_HAL_AVR {
class APM2PPMInput;
class APM1PWMOutput;
class APM2PWMOutput;
class ArduinoScheduler;
}
#endif //__AP_HAL_AVR_NAMESPACE_H__

176
libraries/AP_HAL_AVR/HAL_AVR.cpp
View File

@ -2,181 +2,7 @@
#include "HAL_AVR.h"
using namespace AP_HAL_AVR;
#include <avr/io.h>
#include <avr/interrupt.h>
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
volatile unsigned long timer0_overflow_count = 0;
volatile unsigned long timer0_millis = 0;
static unsigned char timer0_fract = 0;
/**
* HAL_AVR::init is based on the Arduino wiring.c init, but specialized for
* the atmega2560
*/
void HAL_AVR::init(void* opts) const {
// 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);
// 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
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;
scheduler->init();
};
#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)
unsigned long m = timer0_millis;
unsigned char 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++;
}
unsigned long millis()
{
unsigned long 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;
}
unsigned long micros() {
unsigned long m;
uint8_t oldSREG = SREG, t;
cli();
m = timer0_overflow_count;
t = TCNT0;
if ((TIFR0 & _BV(TOV0)) && (t < 255))
m++;
SREG = oldSREG;
return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond());
}
void delay(unsigned long ms)
{
uint16_t start = (uint16_t)micros();
while (ms > 0) {
if (((uint16_t)micros() - start) >= 1000) {
ms--;
start += 1000;
}
}
}
/* Delay for the given number of microseconds. Assumes a 8 or 16 MHz clock. */
void delayMicroseconds(unsigned int 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)
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
);
}

6
libraries/AP_HAL_AVR/HAL_AVR.h
View File

@ -24,10 +24,12 @@ public:
AP_HAL::Console* _console,
AP_HAL::GPIO* _gpio,
AP_HAL::PPMInput* _ppmin,
AP_HAL::PWMOutput* _pwmout)
AP_HAL::PWMOutput* _pwmout,
AP_HAL::Scheduler* _scheduler)
: AP_HAL::HAL( _uart0, _uart1, _uart2, _uart3,
_i2c, _spi, _analogIn, _storage,
_log, _console, _gpio, _ppmin, _pwmout) {}
_log, _console, _gpio, _ppmin,
_pwmout, _scheduler) {}
void init(void* opts) const;
};

180
libraries/AP_HAL_AVR/Scheduler.cpp Normal file
View File

@ -0,0 +1,180 @@
#include "HAL_AVR.h"
#include "Scheduler.h"
using namespace AP_HAL_AVR;
#include <avr/io.h>
#include <avr/interrupt.h>
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
static volatile unsigned long timer0_overflow_count = 0;
static volatile unsigned long timer0_millis = 0;
static unsigned char timer0_fract = 0;
void ArduinoScheduler::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);
// 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
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)
unsigned long m = timer0_millis;
unsigned char 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++;
}
unsigned long ArduinoScheduler::millis()
{
unsigned long 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;
}
unsigned long ArduinoScheduler::micros() {
unsigned long m;
uint8_t oldSREG = SREG, t;
cli();
m = timer0_overflow_count;
t = TCNT0;
if ((TIFR0 & _BV(TOV0)) && (t < 255))
m++;
SREG = oldSREG;
return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond());
}
void ArduinoScheduler::delay(unsigned long ms)
{
uint16_t start = (uint16_t)micros();
while (ms > 0) {
if (((uint16_t)micros() - start) >= 1000) {
ms--;
start += 1000;
}
}
}
/* Delay for the given number of microseconds. Assumes a 8 or 16 MHz clock. */
void ArduinoScheduler::delayMicroseconds(unsigned int 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)
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
);
}

20
libraries/AP_HAL_AVR/Scheduler.h Normal file
View File

@ -0,0 +1,20 @@
#ifndef __AP_HAL_AVR_SCHEDULER_H__
#define __AP_HAL_AVR_SCHEDULER_H__
#include <AP_HAL.h>
#include "AP_HAL_AVR_Namespace.h"
class AP_HAL_AVR::ArduinoScheduler : public AP_HAL::Scheduler {
public:
ArduinoScheduler() {}
/* AP_HAL::Scheduler methods */
void init();
void delay(unsigned long ms);
unsigned long millis();
unsigned long micros();
void delayMicroseconds(unsigned int us);
};
#endif // __AP_HAL_AVR_SCHEDULER_H__