From 2046e396a2fd94cf3f8336298121b5cde01a68bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Jun 2012 21:48:34 +1000 Subject: [PATCH] AnalogSource: make the Arduino AnalogSource interrupt driven this fixes several problems with reading analog sources: - we were getting poor values because we didn't wait long enough for an analog source to settle - we wasted a lot of CPU cycles waiting for conversions - we were not taking averages over many samples, which we did with the old AP_ADC driver on the APM1 --- .../AP_AnalogSource/AP_AnalogSource_ADC.h | 4 +- .../AP_AnalogSource_Arduino.cpp | 150 +++++++++++++++++- .../AP_AnalogSource/AP_AnalogSource_Arduino.h | 30 +++- .../AP_PeriodicProcess/AP_TimerProcess.h | 2 +- 4 files changed, 172 insertions(+), 14 deletions(-) diff --git a/libraries/AP_AnalogSource/AP_AnalogSource_ADC.h b/libraries/AP_AnalogSource/AP_AnalogSource_ADC.h index dd8d73dc2c..4dee76ffd5 100644 --- a/libraries/AP_AnalogSource/AP_AnalogSource_ADC.h +++ b/libraries/AP_AnalogSource/AP_AnalogSource_ADC.h @@ -8,13 +8,13 @@ class AP_AnalogSource_ADC : public AP_AnalogSource { public: - AP_AnalogSource_ADC( AP_ADC * adc, int ch, float prescale = 1.0 ) : + AP_AnalogSource_ADC( AP_ADC * adc, uint8_t ch, float prescale = 1.0 ) : _adc(adc), _ch(ch), _prescale(prescale) {} float read(void); private: AP_ADC * _adc; - int _ch; + uint8_t _ch; float _prescale; }; diff --git a/libraries/AP_AnalogSource/AP_AnalogSource_Arduino.cpp b/libraries/AP_AnalogSource/AP_AnalogSource_Arduino.cpp index ff9189e79a..66a16c904e 100644 --- a/libraries/AP_AnalogSource/AP_AnalogSource_Arduino.cpp +++ b/libraries/AP_AnalogSource/AP_AnalogSource_Arduino.cpp @@ -1,15 +1,149 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "wiring.h" -#endif +#include #include "AP_AnalogSource_Arduino.h" +// increase this if we need more than 5 analog sources +#define MAX_PIN_SOURCES 5 + +// number of times to read a pin +// before considering the value valid. +// This ensures the value has settled on +// the new source +#define PIN_READ_REPEAT 2 + +static uint8_t num_pins_watched; +static uint8_t next_pin_index; +static uint8_t next_pin_count; +static volatile struct { + uint8_t pin; + uint8_t sum_count; + uint16_t output; + uint16_t sum; +} pins[MAX_PIN_SOURCES]; + +// ADC conversion timer. This is called at 1kHz by the timer +// interrupt +// each conversion takes about 125 microseconds +static void adc_timer(uint32_t t) +{ + if (bit_is_set(ADCSRA, ADSC) || + num_pins_watched == 0) { + // conversion is still running. This should be + // very rare, as we are called at 1kHz + return; + } + + next_pin_count++; + if (next_pin_count != PIN_READ_REPEAT) { + // we don't want this value, so start the next conversion + // immediately, discarding this value + ADCSRA |= _BV(ADSC); + return; + } + + // remember the value we got + uint8_t low = ADCL; + uint8_t high = ADCH; + pins[next_pin_index].output = low | (high<<8); + pins[next_pin_index].sum += pins[next_pin_index].output; + if (pins[next_pin_index].sum_count >= 63) { + // we risk overflowing the 16 bit sum + pins[next_pin_index].sum >>= 1; + pins[next_pin_index].sum_count = 32; + } else { + pins[next_pin_index].sum_count++; + } + + next_pin_count = 0; + if (num_pins_watched != 0) { + next_pin_index = (next_pin_index+1) % num_pins_watched; + } + uint8_t pin = pins[next_pin_index].pin; + + if (pin == ANALOG_PIN_VCC) { + // we're reading the board voltage + ADMUX = _BV(REFS0)|_BV(MUX4)|_BV(MUX3)|_BV(MUX2)|_BV(MUX1); + } else { + // we're reading an external pin + ADCSRB = (ADCSRB & ~(1 << MUX5)) | (((pin >> 3) & 0x01) << MUX5); + ADMUX = _BV(REFS0) | (pin & 0x07); + } + + // start the next conversion + ADCSRA |= _BV(ADSC); +} + +// setup the timer process. This must be called before any analog +// values are available +void AP_AnalogSource_Arduino::init_timer(AP_PeriodicProcess * scheduler) +{ + scheduler->register_process(adc_timer); +} + +// read raw 16 bit value +uint16_t AP_AnalogSource_Arduino::read_raw(void) +{ + uint16_t ret; + cli(); + ret = pins[_pin_index].output; + sei(); + return ret; +} + +// scaled read for board Vcc +uint16_t AP_AnalogSource_Arduino::read_vcc(void) +{ + return 1126400UL / read_raw(); +} + +// read the average 16 bit value since the last +// time read_average() was called. This gives a very cheap +// filtered value, as new values are produced at 500/N Hz +// where N is the total number of analog sources +uint16_t AP_AnalogSource_Arduino::read_average(void) +{ + uint16_t sum; + uint8_t sum_count; + + // we don't expect this loop to trigger, unless + // you call read_average() very frequently + while (pins[_pin_index].sum_count == 0) ; + + cli(); + sum = pins[_pin_index].sum; + sum_count = pins[_pin_index].sum_count; + pins[_pin_index].sum = 0; + pins[_pin_index].sum_count = 0; + sei(); + return sum / sum_count; +} + +// read with the prescaler. This uses the averaged value since +// the last read, which matches that the AP_ADC APM1 library does +// for ADC sources float AP_AnalogSource_Arduino::read(void) { - float fullscale = analogRead(_pin); - float scaled = _prescale * fullscale; - return scaled; + return read_average() * _prescale; +} + + +// assign a slot in the pins_watched +void AP_AnalogSource_Arduino::assign_pin_index(uint8_t pin) +{ + // ensure we don't try to read from too many analog pins + if (num_pins_watched == MAX_PIN_SOURCES) { + while (true) { + Serial.printf_P(PSTR("MAX_PIN_SOURCES REACHED\n")); + delay(1000); + } + } + _pin_index = num_pins_watched; + pins[_pin_index].pin = pin; + num_pins_watched++; + if (num_pins_watched == 1) { + // enable the ADC + PRR0 &= ~_BV(PRADC); + ADCSRA |= _BV(ADEN); + } } diff --git a/libraries/AP_AnalogSource/AP_AnalogSource_Arduino.h b/libraries/AP_AnalogSource/AP_AnalogSource_Arduino.h index 31eb895501..0012e6db2d 100644 --- a/libraries/AP_AnalogSource/AP_AnalogSource_Arduino.h +++ b/libraries/AP_AnalogSource/AP_AnalogSource_Arduino.h @@ -4,17 +4,41 @@ #define __AP_ANALOG_SOURCE_ARDUINO_H__ #include "AnalogSource.h" +#include "AP_PeriodicProcess.h" + +// special pin number which is interpreted as a +// internal Vcc voltage read +#define ANALOG_PIN_VCC 255 class AP_AnalogSource_Arduino : public AP_AnalogSource { public: - AP_AnalogSource_Arduino( int pin, float prescale = 1.0 ) : - _pin(pin), _prescale(prescale) {} + AP_AnalogSource_Arduino( uint8_t pin, float prescale = 1.0 ) : + _prescale(prescale) { + assign_pin_index(pin); + } + + // setup the timer callback + static void init_timer(AP_PeriodicProcess * scheduler); + + // read a value with a prescale float read(void); + // read the raw 16 bit ADC value + uint16_t read_raw(void); + + // read a Vcc value in millivolts + uint16_t read_vcc(void); + + // read the average 16 bit ADC value since + // we last called read_average(). + uint16_t read_average(void); + private: - int _pin; + uint8_t _pin_index; float _prescale; + + void assign_pin_index(uint8_t pin); }; #endif // __AP_ANALOG_SOURCE_ARDUINO_H__ diff --git a/libraries/AP_PeriodicProcess/AP_TimerProcess.h b/libraries/AP_PeriodicProcess/AP_TimerProcess.h index f90fb3cbef..43b94f0857 100644 --- a/libraries/AP_PeriodicProcess/AP_TimerProcess.h +++ b/libraries/AP_PeriodicProcess/AP_TimerProcess.h @@ -8,7 +8,7 @@ // default to 1kHz timer interrupt #define TIMERPROCESS_PER_DEFAULT (256-62) // 1kHz -#define AP_TIMERPROCESS_MAX_PROCS 3 +#define AP_TIMERPROCESS_MAX_PROCS 4 class AP_TimerProcess : public AP_PeriodicProcess {