From 0c3e59a307979362407716cecc8ea51cdfb78ef8 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Wed, 28 Nov 2012 16:05:11 -0800 Subject: [PATCH] AP_HAL_AVR: AnalogIn channels respect pins, obey NONE input, all common * there's no need for separate APM1/APM2 classes, so that was eliminated * single class only has special member for vcc. all others created by channel interface. --- libraries/AP_HAL_AVR/AP_HAL_AVR.cpp | 7 ++- libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h | 2 - libraries/AP_HAL_AVR/AnalogIn.h | 43 +++++------------ libraries/AP_HAL_AVR/AnalogIn_ADC.cpp | 7 +-- libraries/AP_HAL_AVR/AnalogIn_APM1.cpp | 38 --------------- libraries/AP_HAL_AVR/AnalogIn_APM2.cpp | 50 -------------------- libraries/AP_HAL_AVR/AnalogIn_Common.cpp | 51 ++++++++++++++++++--- 7 files changed, 64 insertions(+), 134 deletions(-) delete mode 100644 libraries/AP_HAL_AVR/AnalogIn_APM1.cpp delete mode 100644 libraries/AP_HAL_AVR/AnalogIn_APM2.cpp diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp b/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp index 176d8fb0d8..c241f68e2c 100644 --- a/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp +++ b/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp @@ -32,8 +32,7 @@ static EmptyUARTDriver emptyUartDriver; static AVRI2CDriver avrI2CDriver; static ArduinoSPIDriver arduinoSPIDriver; -static APM1AnalogIn apm1AnalogIn; -static APM2AnalogIn apm2AnalogIn; +static AVRAnalogIn analogIn; static AVREEPROMStorage avrEEPROMStorage; static APM1Dataflash apm1Dataflash; static APM2Dataflash apm2Dataflash; @@ -52,7 +51,7 @@ const HAL_AVR AP_HAL_AVR_APM1( (UARTDriver*) &avrUart3Driver, &avrI2CDriver, &arduinoSPIDriver, - &apm1AnalogIn, + &analogIn, &avrEEPROMStorage, &apm1Dataflash, &consoleDriver, @@ -68,7 +67,7 @@ const HAL_AVR AP_HAL_AVR_APM2( (UARTDriver*) &emptyUartDriver, &avrI2CDriver, &arduinoSPIDriver, - &apm2AnalogIn, + &analogIn, &avrEEPROMStorage, &apm2Dataflash, &consoleDriver, diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h index 98772b2712..5154304bc9 100644 --- a/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h +++ b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h @@ -10,8 +10,6 @@ namespace AP_HAL_AVR { class ArduinoSPIDriver; class ADCSource; class AVRAnalogIn; - class APM1AnalogIn; - class APM2AnalogIn; class AVREEPROMStorage; class CommonDataflash; class APM1Dataflash; diff --git a/libraries/AP_HAL_AVR/AnalogIn.h b/libraries/AP_HAL_AVR/AnalogIn.h index b189192e80..3b065cbda9 100644 --- a/libraries/AP_HAL_AVR/AnalogIn.h +++ b/libraries/AP_HAL_AVR/AnalogIn.h @@ -5,11 +5,11 @@ #include #include "AP_HAL_AVR_Namespace.h" -#define AVR_ANALOG_PIN_VCC 255 -#define AVR_INPUT_MAX_CHANNELS 6 +#define AVR_INPUT_MAX_CHANNELS 12 class AP_HAL_AVR::ADCSource : public AP_HAL::AnalogSource { public: + friend class AP_HAL_AVR::AVRAnalogIn; /* pin designates the ADC input number, or when == AVR_ANALOG_PIN_VCC, * board vcc */ ADCSource(uint8_t pin, float prescale = 1.0); @@ -29,6 +29,8 @@ public: /* read_average: called to calculate and clear the internal average. * implements read(). */ float read_average(); + + int get_pin() { return _pin; }; private: /* _sum_count and _sum are used from both an interrupt and normal thread */ volatile uint8_t _sum_count; @@ -40,47 +42,26 @@ private: const float _prescale; }; - /* AVRAnalogIn : a concrete class providing the implementations of the - * timer event */ -class AP_HAL_AVR::AVRAnalogIn { + * timer event and the AP_HAL::AnalogIn interface */ +class AP_HAL_AVR::AVRAnalogIn : public AP_HAL::AnalogIn { public: AVRAnalogIn(); + void init(void* ap_hal_scheduler); + AP_HAL::AnalogSource* channel(int n); + protected: + static ADCSource* _find_or_create_channel(int num); + static ADCSource* _create_channel(int num); static void _register_channel(ADCSource*); static void _timer_event(uint32_t); static ADCSource* _channels[AVR_INPUT_MAX_CHANNELS]; static int _num_channels; static int _active_channel; static int _channel_repeat_count; -}; -/* APM1AnalogIn and APM2AnalogIn match the implementations in the AVRAnalogIn - * class with the AP_HAL::AnalogIn's channel interface. */ -class AP_HAL_AVR::APM1AnalogIn : public AP_HAL::AnalogIn, - public AP_HAL_AVR::AVRAnalogIn { -public: - APM1AnalogIn(); - void init(void* ap_hal_scheduler); - AP_HAL::AnalogSource* channel(int n); private: - ADCSource _bat_voltage; - ADCSource _bat_current; - ADCSource _vdd; + ADCSource _vcc; }; -class AP_HAL_AVR::APM2AnalogIn : public AP_HAL::AnalogIn, - public AP_HAL_AVR::AVRAnalogIn { -public: - APM2AnalogIn(); - void init(void* ap_hal_scheduler); - AP_HAL::AnalogSource* channel(int n); -private: - ADCSource _pitot; - ADCSource _bat_voltage; - ADCSource _bat_current; - ADCSource _rssi; - ADCSource _vdd; -}; #endif // __AP_HAL_AVR_ANALOG_IN_H__ - diff --git a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp index 24202cbcb5..16aecba90c 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_ADC.cpp @@ -27,7 +27,7 @@ float ADCSource::read_average() { * have a way to bubble control upwards at the moment. -pch */ while( _sum_count == 0 ); - /* Read and clear the */ + /* Read and clear in a critical section */ cli(); sum = _sum; sum_count = _sum_count; @@ -40,15 +40,16 @@ float ADCSource::read_average() { } void ADCSource::setup_read() { - if (_pin == AVR_ANALOG_PIN_VCC) { + if (_pin == ANALOG_INPUT_BOARD_VCC) { ADMUX = _BV(REFS0)|_BV(MUX4)|_BV(MUX3)|_BV(MUX2)|_BV(MUX1); + } else if (_pin == ANALOG_INPUT_NONE) { + /* noop */ } else { ADCSRB = (ADCSRB & ~(1 << MUX5)) | (((_pin >> 3) & 0x01) << MUX5); ADMUX = _BV(REFS0) | (_pin & 0x07); } } - /* new_sample is called from an interrupt. It always has access to * _sum and _sum_count. Lock out the interrupts briefly with * cli/sei to read these variables from outside an interrupt. */ diff --git a/libraries/AP_HAL_AVR/AnalogIn_APM1.cpp b/libraries/AP_HAL_AVR/AnalogIn_APM1.cpp deleted file mode 100644 index 9f05e78c01..0000000000 --- a/libraries/AP_HAL_AVR/AnalogIn_APM1.cpp +++ /dev/null @@ -1,38 +0,0 @@ - -#include -#include "AnalogIn.h" -using namespace AP_HAL_AVR; - -extern const AP_HAL::HAL& hal; - -APM1AnalogIn::APM1AnalogIn () : - _bat_voltage(ADCSource(0)), - _bat_current(ADCSource(1)), - _vdd(ADCSource(AVR_ANALOG_PIN_VCC)), - AVRAnalogIn() -{ } - -void APM1AnalogIn::init(void* machtnichts) { - /* Register AVRAnalogIn::_timer_event with the scheduler. */ - hal.scheduler->register_timer_process(_timer_event); - /* Register each private channel with AVRAnalogIn. */ - _register_channel(&_bat_voltage); - _register_channel(&_bat_current); - _register_channel(&_vdd); -} - -AP_HAL::AnalogSource* APM1AnalogIn::channel(int ch) { - switch(ch) { - case 0: - return &_bat_voltage; - break; - case 1: - return &_bat_current; - break; - case 2: - return &_vdd; - break; - default: - return NULL; - } -} diff --git a/libraries/AP_HAL_AVR/AnalogIn_APM2.cpp b/libraries/AP_HAL_AVR/AnalogIn_APM2.cpp deleted file mode 100644 index b9b6243aba..0000000000 --- a/libraries/AP_HAL_AVR/AnalogIn_APM2.cpp +++ /dev/null @@ -1,50 +0,0 @@ - -#include - -#include "AnalogIn.h" -using namespace AP_HAL_AVR; - -extern const AP_HAL::HAL &hal; - -APM2AnalogIn::APM2AnalogIn () : - _pitot(ADCSource(0, 4.0)), - _bat_voltage(ADCSource(1)), - _bat_current(ADCSource(2)), - _rssi(ADCSource(3, 0.25)), - _vdd(ADCSource(AVR_ANALOG_PIN_VCC)), - AVRAnalogIn() -{ } - -void APM2AnalogIn::init(void * machtichts) { - /* Register AVRAnalogIn::_timer_event with the scheduler. */ - hal.scheduler->register_timer_process(_timer_event); - /* Register each private channel with AVRAnalogIn. */ - _register_channel(&_pitot); - _register_channel(&_bat_voltage); - _register_channel(&_bat_current); - _register_channel(&_rssi); - _register_channel(&_vdd); -} - -AP_HAL::AnalogSource* APM2AnalogIn::channel(int ch) { - switch(ch) { - case 0: - return &_bat_voltage; - break; - case 1: - return &_bat_current; - break; - case 2: - return &_vdd; - break; - case 3: - return &_pitot; - break; - case 4: - return &_rssi; - break; - default: - return NULL; - } -} - diff --git a/libraries/AP_HAL_AVR/AnalogIn_Common.cpp b/libraries/AP_HAL_AVR/AnalogIn_Common.cpp index 66576a621d..1d7cc0e892 100644 --- a/libraries/AP_HAL_AVR/AnalogIn_Common.cpp +++ b/libraries/AP_HAL_AVR/AnalogIn_Common.cpp @@ -18,7 +18,31 @@ int AVRAnalogIn::_num_channels = 0; int AVRAnalogIn::_active_channel = 0; int AVRAnalogIn::_channel_repeat_count = 0; -AVRAnalogIn::AVRAnalogIn() {} +AVRAnalogIn::AVRAnalogIn() : + _vcc(ADCSource(ANALOG_INPUT_BOARD_VCC)) +{} + +void AVRAnalogIn::init(void* machtnichts) { + /* Register AVRAnalogIn::_timer_event with the scheduler. */ + hal.scheduler->register_timer_process(_timer_event); + /* Register each private channel with AVRAnalogIn. */ + _register_channel(&_vcc); +} + +ADCSource* AVRAnalogIn::_find_or_create_channel(int pin) { + for(int i = 0; i < _num_channels; i++) { + if (_channels[i]->_pin == pin) { + return _channels[i]; + } + } + return _create_channel(pin); +} + +ADCSource* AVRAnalogIn::_create_channel(int chnum) { + ADCSource *ch = new ADCSource(chnum); + _register_channel(ch); + return ch; +} void AVRAnalogIn::_register_channel(ADCSource* ch) { if (_num_channels >= AVR_INPUT_MAX_CHANNELS) { @@ -43,6 +67,11 @@ void AVRAnalogIn::_register_channel(ADCSource* ch) { } void AVRAnalogIn::_timer_event(uint32_t t) { + + if (_channels[_active_channel]->_pin == ANALOG_INPUT_NONE) { + goto next_channel; + } + if (ADCSRA & _BV(ADSC)) { /* ADC Conversion is still running - this should not happen, as we * are called at 1khz. */ @@ -64,11 +93,14 @@ void AVRAnalogIn::_timer_event(uint32_t t) { } /* Read the conversion registers. */ - uint8_t low = ADCL; - uint8_t high = ADCH; - uint16_t sample = low | (((uint16_t)high) << 8); - /* Give the active channel a new sample */ - _channels[_active_channel]->new_sample( sample ); + { + uint8_t low = ADCL; + uint8_t high = ADCH; + uint16_t sample = low | (((uint16_t)high) << 8); + /* Give the active channel a new sample */ + _channels[_active_channel]->new_sample( sample ); + } +next_channel: /* Move to the next channel */ _active_channel = (_active_channel + 1) % _num_channels; /* Setup the next channel's conversion */ @@ -78,4 +110,11 @@ void AVRAnalogIn::_timer_event(uint32_t t) { } +AP_HAL::AnalogSource* AVRAnalogIn::channel(int ch) { + if (ch == ANALOG_INPUT_BOARD_VCC) { + return &_vcc; + } else { + return _find_or_create_channel(ch); + } +}