diff --git a/libraries/AP_HAL/AP_HAL.h b/libraries/AP_HAL/AP_HAL.h index 65febcfe4f..ddd84d1efd 100644 --- a/libraries/AP_HAL/AP_HAL.h +++ b/libraries/AP_HAL/AP_HAL.h @@ -15,6 +15,7 @@ #include "GPIO.h" #include "PPMInput.h" #include "PWMOutput.h" +#include "Scheduler.h" #include "utility/Print.h" #include "utility/Stream.h" diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h index d143c6021d..7ab117f72b 100644 --- a/libraries/AP_HAL/AP_HAL_Namespace.h +++ b/libraries/AP_HAL/AP_HAL_Namespace.h @@ -18,6 +18,7 @@ namespace AP_HAL { class GPIO; class PPMInput; class PWMOutput; + class Scheduler; class EmptyUARTDriver; diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index 19593e1c1f..558da47dd3 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -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__ diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h new file mode 100644 index 0000000000..385d60721d --- /dev/null +++ b/libraries/AP_HAL/Scheduler.h @@ -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__ + diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp b/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp index a68bf7a267..bfc8388a23 100644 --- a/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp +++ b/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp @@ -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 ); diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h index a2099630c5..03e1ef292f 100644 --- a/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h +++ b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h @@ -18,6 +18,7 @@ namespace AP_HAL_AVR { class APM2PPMInput; class APM1PWMOutput; class APM2PWMOutput; + class ArduinoScheduler; } #endif //__AP_HAL_AVR_NAMESPACE_H__ diff --git a/libraries/AP_HAL_AVR/HAL_AVR.cpp b/libraries/AP_HAL_AVR/HAL_AVR.cpp index f04a2509c0..123b108c13 100644 --- a/libraries/AP_HAL_AVR/HAL_AVR.cpp +++ b/libraries/AP_HAL_AVR/HAL_AVR.cpp @@ -2,181 +2,7 @@ #include "HAL_AVR.h" using namespace AP_HAL_AVR; -#include -#include - -#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 - ); -} - diff --git a/libraries/AP_HAL_AVR/HAL_AVR.h b/libraries/AP_HAL_AVR/HAL_AVR.h index 617b2aa0fa..b2617924a5 100644 --- a/libraries/AP_HAL_AVR/HAL_AVR.h +++ b/libraries/AP_HAL_AVR/HAL_AVR.h @@ -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; }; diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp new file mode 100644 index 0000000000..6288246858 --- /dev/null +++ b/libraries/AP_HAL_AVR/Scheduler.cpp @@ -0,0 +1,180 @@ + +#include "HAL_AVR.h" +#include "Scheduler.h" +using namespace AP_HAL_AVR; + +#include +#include + + +#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 + ); +} + diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h new file mode 100644 index 0000000000..367462050f --- /dev/null +++ b/libraries/AP_HAL_AVR/Scheduler.h @@ -0,0 +1,20 @@ + +#ifndef __AP_HAL_AVR_SCHEDULER_H__ +#define __AP_HAL_AVR_SCHEDULER_H__ + +#include +#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__ +