AP_HAL_AVR: implement ::init method in derived HAL_AVR class.

* Implementation of ::init taken from Arduino core. Not tested yet

libraries/AP_HAL_AVR/AP_HAL_AVR.cpp

@@ -3,6 +3,7 @@
 #include "AP_HAL_AVR.h"
 
 /* Include AVR-specific implementations of the HAL classes */
+#include "HAL_AVR.h"
 #include "UARTDriver.h"
 #include "I2CDriver.h"
 #include "SPIDriver.h"
@@ -39,7 +40,7 @@ static APM2PPMInput     apm2PPMInput;
 static APM1PWMOutput    apm1PWMOutput;
 static APM2PWMOutput    apm2PWMOutput;
 
-const HAL AP_HAL_AVR_APM1(
+const HAL_AVR AP_HAL_AVR_APM1(
         (UARTDriver*) &avrUart0Driver,
         (UARTDriver*) &avrUart1Driver,
         (UARTDriver*) &avrUart2Driver,
@@ -54,7 +55,7 @@ const HAL AP_HAL_AVR_APM1(
         &apm1PPMInput,
         &apm1PWMOutput );
 
-const HAL AP_HAL_AVR_APM2(
+const HAL_AVR AP_HAL_AVR_APM2(
         (UARTDriver*) &avrUart0Driver,
         (UARTDriver*) &avrUart1Driver,
         (UARTDriver*) &avrUart2Driver,

libraries/AP_HAL_AVR/AP_HAL_AVR.h

@@ -3,6 +3,7 @@
 #define __AP_HAL_AVR_H__
 
 #include <AP_HAL.h>
+#include "HAL_AVR.h"
 
 /**
  * This module exports AP_HAL instances only.
@@ -10,8 +11,8 @@
  * and not expose implementation details.
  */
 
-extern const AP_HAL::HAL AP_HAL_AVR_APM1;
-extern const AP_HAL::HAL AP_HAL_AVR_APM2;
+extern const AP_HAL_AVR::HAL_AVR AP_HAL_AVR_APM1;
+extern const AP_HAL_AVR::HAL_AVR AP_HAL_AVR_APM2;
 
 #endif // __AP_HAL_AVR_H__
 

libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h

@@ -3,6 +3,8 @@
 #define __AP_HAL_AVR_NAMESPACE_H__
 
 namespace AP_HAL_AVR {
+    class HAL_AVR;
+
     class AVRUARTDriver;
     class AVRI2CDriver;
     class ArduinoSPIDriver;

libraries/AP_HAL_AVR/HAL_AVR.cpp

@@ -0,0 +1,182 @@
+
+#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;
+};
+
+#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

@@ -0,0 +1,35 @@
+
+#ifndef __AP_HAL_AVR_HAL_AVR_H__
+#define __AP_HAL_AVR_HAL_AVR_H__
+
+#include <AP_HAL.h>
+#include "AP_HAL_AVR_Namespace.h"
+
+/* HAL_AVR class derives from HAL but provides an AVR-specific
+ * init method.
+ */
+
+class AP_HAL_AVR::HAL_AVR : public AP_HAL::HAL {
+public:
+    HAL_AVR(
+        AP_HAL::UARTDriver* _uart0,
+        AP_HAL::UARTDriver* _uart1,
+        AP_HAL::UARTDriver* _uart2,
+        AP_HAL::UARTDriver* _uart3,
+        AP_HAL::I2CDriver*  _i2c,
+        AP_HAL::SPIDriver*  _spi,
+        AP_HAL::AnalogIn*   _analogIn,
+        AP_HAL::Storage*    _storage,
+        AP_HAL::Log*        _log,
+        AP_HAL::Console*    _console,
+        AP_HAL::GPIO*       _gpio,
+        AP_HAL::PPMInput*   _ppmin,
+        AP_HAL::PWMOutput*  _pwmout)
+        : AP_HAL::HAL(  _uart0, _uart1, _uart2, _uart3,
+                        _i2c, _spi, _analogIn, _storage,
+                        _log, _console, _gpio, _ppmin, _pwmout) {}
+
+    void init(void* opts) const;
+};
+#endif // __AP_HAL_AVR_HAL_AVR_H__
+

libraries/AP_HAL_AVR/examples/APM1/APM1.pde

@@ -3,7 +3,7 @@
 #include <AP_HAL.h>
 #include <AP_HAL_AVR.h>
 
-const AP_HAL::HAL& hal1 = AP_HAL_AVR_APM1;
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
 const AP_HAL::HAL& hal2 = AP_HAL_AVR_APM2;
 
 void loop (void) {
@@ -11,14 +11,22 @@ void loop (void) {
 }
 
 void setup (void) {
-    hal1.uart0->begin(9600);
+    hal.uart0->begin(9600);
     hal2.uart0->begin(9600);
-    hal1.uart1->begin(9600);
+    hal.uart1->begin(9600);
     hal2.uart1->begin(9600);
-    hal1.uart2->begin(9600);
+    hal.uart2->begin(9600);
     hal2.uart2->begin(9600);
-    hal1.uart3->begin(9600);
+    hal.uart3->begin(9600);
     hal2.uart3->begin(9600);
 }
 
 
+extern "C" {
+int main (void) {
+    hal.init(NULL);
+    setup();
+    for(;;) loop();
+    return 0;
+}
+}