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

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"

libraries/AP_HAL/AP_HAL_Namespace.h

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

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__

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__
+

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 );
 

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__

libraries/AP_HAL_AVR/HAL_AVR.cpp

@@ -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
+    scheduler->init();
-    // 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 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
-	);
-}
-

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;
 };

libraries/AP_HAL_AVR/Scheduler.cpp

@@ -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
+	);
+}
+

libraries/AP_HAL_AVR/Scheduler.h

@@ -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__
+