AP_HAL_AVR: Improved AVRTimer micros() and millis()

- More efficient code by using 16-bit timer
- micros() now has proper 1 us resolution and less overhead
- millis() has less overhead
- removed unneeded/unwanted initializatin of timers in AVRTimer::init()

pull request 62, approved and merged by pat

libraries/AP_HAL_AVR/RCInput_APM1.cpp

@@ -64,7 +64,7 @@ void APM1RCInput::init(void* _isrregistry) {
      */
     TCCR4A = _BV(WGM40) | _BV(WGM41);
     TCCR4B = _BV(WGM43) | _BV(WGM42) | _BV(CS41) | _BV(ICES4);
-    OCR4A  = 40000;
+    OCR4A  = 40000 - 1; // -1 to correct for wrap
 
     /* OCR4B and OCR4C will be used by RCOutput_APM1. init to nil output */
     OCR4B  = 0xFFFF;
@@ -72,6 +72,9 @@ void APM1RCInput::init(void* _isrregistry) {
 
     /* Enable input capture interrupt */
     TIMSK4 |= _BV(ICIE4);
+    
+    /* Enable overflow interrupt */
+    TIMSK4 |= _BV(TOIE4);
 }
 
 uint8_t APM1RCInput::valid() { return _valid; }

libraries/AP_HAL_AVR/RCInput_APM2.cpp

@@ -64,7 +64,7 @@ void APM2RCInput::init(void* _isrregistry) {
      */
     TCCR5A = _BV(WGM50) | _BV(WGM51);
     TCCR5B = _BV(WGM53) | _BV(WGM52) | _BV(CS51) | _BV(ICES5);
-    OCR5A  = 40000;
+    OCR5A  = 40000 - 1; // -1 to correct for wrap
 
     /* OCR5B and OCR5C will be used by RCOutput_APM2. init to nil output */
     OCR5B  = 0xFFFF;
@@ -72,6 +72,9 @@ void APM2RCInput::init(void* _isrregistry) {
 
     /* Enable input capture interrupt */
     TIMSK5 |= _BV(ICIE5);
+    
+    /* Enable overflow interrupt */
+    TIMSK5 |= _BV(TOIE5);
 }
 
 uint8_t APM2RCInput::valid() { return _valid; }

libraries/AP_HAL_AVR/Scheduler_Timer.cpp

@@ -10,48 +10,14 @@ using namespace AP_HAL_AVR;
 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
 #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
 
-static volatile uint32_t timer0_overflow_count = 0;
-static volatile uint32_t timer0_millis = 0;
-static uint8_t timer0_fract = 0;
-
+static volatile uint32_t timer_micros_counter = 0;
+static volatile uint32_t timer_millis_counter = 0;
 
 void AVRTimer::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);
-
-    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
@@ -69,74 +35,42 @@ void AVRTimer::init() {
     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)
-
+#if (CONFIG_HAL_BOARD == HAL_BOARD_APM1 )
+#define  AVR_TIMER_OVF_VECT     TIMER4_OVF_vect
+#define  AVR_TIMER_TCNT             TCNT4
+#elif (CONFIG_HAL_BOARD == HAL_BOARD_APM2 )
+#define  AVR_TIMER_OVF_VECT     TIMER5_OVF_vect 
+#define  AVR_TIMER_TCNT             TCNT5
+#endif
 
-SIGNAL(TIMER0_OVF_vect)
+SIGNAL( AVR_TIMER_OVF_VECT)
 {
-	// copy these to local variables so they can be stored in registers
-	// (volatile variables must be read from memory on every access)
-	uint32_t m = timer0_millis;
-	uint8_t f = timer0_fract;
-
-	m += MILLIS_INC;
-	f += FRACT_INC;
-	if (f >= FRACT_MAX) {
-		f -= FRACT_MAX;
-		m += 1;
+    // 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
 }
 
-	timer0_fract = f;
-	timer0_millis = m;
-	timer0_overflow_count++;
-}
-
-uint32_t AVRTimer::millis()
-{
-	uint32_t m;
+uint32_t AVRTimer::micros() {
     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;
+    // 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);
     SREG = oldSREG;
-
-	return m;
+	return  time_micros;
 }
 
-uint32_t AVRTimer::micros() {
-	uint32_t m;
-    uint8_t t;
-	
+uint32_t AVRTimer::millis() {
 	uint8_t oldSREG = SREG;
 	cli();
-
-	m = timer0_overflow_count;
-	t = TCNT0;
-  
-	if ((TIFR0 & _BV(TOV0)) && (t < 255))
-		m++;
-
+    // 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)
     SREG = oldSREG;
-	
-	return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond());
+	return  time_millis;
 }
 
+
 /* Delay for the given number of microseconds.  Assumes a 16 MHz clock. */
 void AVRTimer::delay_microseconds(uint16_t us)
 {