From 8a31af801fa4d7fe54ef0d2b6e4398f007c99e46 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 13:01:45 +1000 Subject: [PATCH] ADC: re-work for ADC code for full resolution and accurate timing this changes the ADC code to return the full resolution of the sensors. It also adds a new Ch6() interface that returns 6 channels at once, so the IMU can read 3 accelerometers and 3 gyros at once, and get the exact time that the values were accumulated over --- libraries/AP_ADC/AP_ADC.h | 26 ++++-- libraries/AP_ADC/AP_ADC_ADS7844.cpp | 123 +++++++++++++++++++--------- libraries/AP_ADC/AP_ADC_ADS7844.h | 8 +- libraries/AP_ADC/AP_ADC_HIL.cpp | 16 ++-- libraries/AP_ADC/AP_ADC_HIL.h | 20 +++-- 5 files changed, 133 insertions(+), 60 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC.h b/libraries/AP_ADC/AP_ADC.h index 196a6741c3..cafc460c1a 100644 --- a/libraries/AP_ADC/AP_ADC.h +++ b/libraries/AP_ADC/AP_ADC.h @@ -1,6 +1,8 @@ #ifndef AP_ADC_H #define AP_ADC_H +#include + /* AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega Code by James Goppert. DIYDrones.com @@ -13,7 +15,7 @@ Methods: Init() : Initialization of ADC. (interrupts etc) Ch(ch_num) : Return the ADC channel value - + Ch6(channel_numbers, result) : Return 6 ADC channel values */ class AP_ADC @@ -21,11 +23,23 @@ class AP_ADC public: AP_ADC() {}; // Constructor virtual void Init() {}; - virtual int Ch(unsigned char ch_num) = 0; - virtual int Ch_raw(unsigned char ch_num) = 0; - private: -}; - + + /* read one channel value */ + virtual uint16_t Ch(uint8_t ch_num) = 0; + + /* read 6 channels values as a set, used by IMU for 3 gyros + and 3 accelerometeres. + + Pass in an array of 6 channel numbers and results are + returned in result[] + + The function returns the amount of time that the returned + value has been averaged over, in 2.5 millisecond units + */ + virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0; + private: +}; + #include "AP_ADC_ADS7844.h" #include "AP_ADC_HIL.h" diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index 27bbc0a477..1625809755 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -44,6 +44,7 @@ extern "C" { // AVR LibC Includes #include + #include #include #include "WConstants.h" } @@ -53,8 +54,12 @@ extern "C" { // Commands for reading ADC channels on ADS7844 static const unsigned char adc_cmd[9] = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 }; -static volatile uint16_t _filter[8][ADC_FILTER_SIZE]; -static volatile uint8_t _filter_index; + +// the sum of the values since last read +static volatile uint32_t _sum[8]; + +// how many values we've accumulated since last read +static volatile uint16_t _count[8]; static unsigned char ADC_SPI_transfer(unsigned char data) { @@ -72,32 +77,33 @@ static unsigned char ADC_SPI_transfer(unsigned char data) ISR (TIMER2_OVF_vect) { uint8_t ch; - uint16_t adc_tmp; - //bit_set(PORTL,6); // To test performance + //bit_set(PORTL,6); // To test performance - bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) - ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel + bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) + ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel for (ch = 0; ch < 8; ch++){ - adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte + uint16_t adc_tmp; + adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command - // Fill our Moving average filter - _filter[ch][_filter_index] = adc_tmp >> 3; + _count[ch]++; + if (_count[ch] == 0) { + // overflow ... shouldn't happen too often + // unless we're just not using the + // channel. Notice that we overflow the count + // to 1 here, not zero, as otherwise the + // reader below could get a division by zero + _sum[ch] = 0; + _count[ch] = 1; + } + _sum[ch] += adc_tmp; } - // increment our filter - _filter_index++; - - // loop our filter - if(_filter_index == ADC_FILTER_SIZE) - _filter_index = 0; - - - bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) - //bit_clear(PORTL,6); // To test performance - TCNT2 = 104; // 400 Hz + bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) + //bit_clear(PORTL,6); // To test performance + TCNT2 = 104; // 400Hz interrupt } @@ -123,6 +129,16 @@ void AP_ADC_ADS7844::Init(void) // Set Baud rate UBRR2 = 2; // SPI clock running at 2.6MHz + // get an initial value for each channel. This ensures + // _count[] is never zero + for (uint8_t i=0; i<8; i++) { + uint16_t adc_tmp; + adc_tmp = ADC_SPI_transfer(0) << 8; + adc_tmp |= ADC_SPI_transfer(adc_cmd[i + 1]); + _count[i] = 1; + _sum[i] = adc_tmp; + } + // Enable Timer2 Overflow interrupt to capture ADC data TIMSK2 = 0; // Disable interrupts @@ -134,31 +150,60 @@ void AP_ADC_ADS7844::Init(void) } // Read one channel value -int AP_ADC_ADS7844::Ch(unsigned char ch_num) +uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num) { - uint16_t result = 0; + uint16_t count; + uint32_t sum; - //while(adc_counter[ch_num] < 2) { } // Wait for at least 2 samples in accumlator + // ensure we have at least one value + while (_count[ch_num] == 0) /* noop */ ; - // stop interrupts + // grab the value with interrupts disabled, and clear the count cli(); - - // sum our filter - for(uint8_t i = 0; i < ADC_FILTER_SIZE; i++){ - result += _filter[ch_num][i]; + count = _count[ch_num]; + sum = _sum[ch_num]; + _count[ch_num] = 0; + _sum[ch_num] = 0; + sei(); + + return sum/count; +} + +// Read 6 channel values +// this assumes that the counts for all of the 6 channels are +// equal. This will only be true if we always consistently access a +// sensor by either Ch6() or Ch() and never mix them. If you mix them +// then you will get very strange results +uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) +{ + uint16_t count[6]; + uint32_t sum[6]; + uint8_t i; + + // ensure we have at least one value + for (i=0; i<6; i++) { + while (_count[channel_numbers[i]] == 0) /* noop */; } - // resume interrupts + // grab the values with interrupts disabled, and clear the counts + cli(); + for (i=0; i<6; i++) { + count[i] = _count[channel_numbers[i]]; + sum[i] = _sum[channel_numbers[i]]; + _count[channel_numbers[i]] = 0; + _sum[channel_numbers[i]] = 0; + } sei(); - // average our sampels - result /= ADC_FILTER_SIZE; - - return(result); -} - -// Read one channel value -int AP_ADC_ADS7844::Ch_raw(unsigned char ch_num) -{ - return _filter[ch_num][_filter_index]; // close enough + // calculate averages. We keep this out of the cli region + // to prevent us stalling the ISR while doing the + // division. That costs us 36 bytes of stack, but I think its + // worth it. + for (i=0; i<6; i++) { + result[i] = sum[i] / count[i]; + } + + // this assumes the count in all channels is equal, which will + // be true if the callers are using the interface consistently + return count[0]; } diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index ca748e4e1f..c4b99ecaed 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -19,8 +19,12 @@ class AP_ADC_ADS7844 : public AP_ADC public: AP_ADC_ADS7844(); // Constructor void Init(); - int Ch(unsigned char ch_num); - int Ch_raw(unsigned char ch_num); + + // Read 1 sensor value + uint16_t Ch(unsigned char ch_num); + + // Read 6 sensors at once + uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result); private: }; diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp index 07cfdbd8f0..1684b161b7 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ b/libraries/AP_ADC/AP_ADC_HIL.cpp @@ -35,6 +35,8 @@ AP_ADC_HIL::AP_ADC_HIL() // set diff press and temp to zero setGyroTemp(0); setPressure(0); + + last_hil_time = millis(); } void AP_ADC_HIL::Init(void) @@ -42,14 +44,18 @@ void AP_ADC_HIL::Init(void) } // Read one channel value -int AP_ADC_HIL::Ch(unsigned char ch_num) +uint16_t AP_ADC_HIL::Ch(unsigned char ch_num) { - return adcValue[ch_num]; + return adcValue[ch_num]; } -// Read one channel value -int AP_ADC_HIL::Ch_raw(unsigned char ch_num) + +// Read 6 channel values +uint16_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result) { - return adcValue[ch_num]; + for (uint8_t i=0; i<6; i++) { + result[i] = Ch(channel_numbers[i]); + } + return ((millis() - last_hil_time)*2)/5; } // Set one channel value diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index 736e99a882..ed0c039289 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -32,10 +32,11 @@ class AP_ADC_HIL : public AP_ADC /// // Read the sensor, part of public AP_ADC interface - int Ch(unsigned char ch_num); + uint16_t Ch(unsigned char ch_num); + /// - // Read the sensor, part of public AP_ADC interface - int Ch_raw(unsigned char ch_num); + // Read 6 sensors at once + uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result); /// // Set the adc raw values given the current rotations rates, @@ -49,16 +50,19 @@ class AP_ADC_HIL : public AP_ADC // The raw adc array uint16_t adcValue[8]; + // the time in milliseconds when we last got a HIL update + uint32_t last_hil_time; + /// // sensor constants // constants declared in cpp file // @see AP_ADC_HIL.cpp - static const uint8_t sensors[6]; - static const float gyroBias[3]; - static const float gyroScale[3]; + static const uint8_t sensors[6]; + static const float gyroBias[3]; + static const float gyroScale[3]; static const float accelBias[3]; - static const float accelScale[3]; - static const int8_t sensorSign[6]; + static const float accelScale[3]; + static const int8_t sensorSign[6]; /// // gyro set function