Revert "AP_HAL_AVR: Improved AVRTimer micros() and millis()"

This reverts commit 527dcdf3.

This was causing the MPU6000 startup code to fail, due to time running
backwards.

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 - 1; // -1 to correct for wrap
+    OCR4A  = 40000;
 
     /* OCR4B and OCR4C will be used by RCOutput_APM1. init to nil output */
     OCR4B  = 0xFFFF;
@@ -72,9 +72,6 @@ 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 - 1; // -1 to correct for wrap
+    OCR5A  = 40000;
 
     /* OCR5B and OCR5C will be used by RCOutput_APM2. init to nil output */
     OCR5B  = 0xFFFF;
@@ -72,9 +72,6 @@ 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,13 +10,47 @@ 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 timer_micros_counter = 0;
-static volatile uint32_t timer_millis_counter = 0;
+static volatile uint32_t timer0_overflow_count = 0;
+static volatile uint32_t timer0_millis = 0;
+static uint8_t timer0_fract = 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.
@@ -35,41 +69,73 @@ void AVRTimer::init() {
     UCSR0B = 0;
 }
 
-#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
+#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)
 
-SIGNAL( AVR_TIMER_OVF_VECT)
+// 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)
 {
-    // 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
+	// 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;
+	}
+
+	timer0_fract = f;
+	timer0_millis = m;
+	timer0_overflow_count++;
 }
 
-uint32_t AVRTimer::micros() {
-    uint8_t oldSREG = SREG;
+uint32_t AVRTimer::millis()
+{
+	uint32_t 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();
-    // 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  time_micros;
+	m = timer0_millis;
+	SREG = oldSREG;
+
+	return m;
 }
 
-uint32_t AVRTimer::millis() {
+uint32_t AVRTimer::micros() {
+	uint32_t m;
+    uint8_t t;
+	
 	uint8_t oldSREG = SREG;
 	cli();
-    // 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  time_millis;
-}
 
+	m = timer0_overflow_count;
+	t = TCNT0;
+  
+	if ((TIFR0 & _BV(TOV0)) && (t < 255))
+		m++;
+
+	SREG = oldSREG;
+	
+	return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond());
+}
 
 /* Delay for the given number of microseconds.  Assumes a 16 MHz clock. */
 void AVRTimer::delay_microseconds(uint16_t us)