From 1c5e8f0381916a203050a7ec6d4aa950605ced07 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 18:45:51 +1000 Subject: [PATCH] change ADC sample rate to 1kHz this costs us about 9% of our CPU, but should make aliasing much less of a problem. --- libraries/AP_ADC/AP_ADC.h | 5 +- libraries/AP_ADC/AP_ADC_ADS7844.cpp | 134 +++++++++++++++------------- libraries/AP_ADC/AP_ADC_ADS7844.h | 8 +- libraries/AP_ADC/AP_ADC_HIL.cpp | 6 +- libraries/AP_ADC/AP_ADC_HIL.h | 10 +-- libraries/AP_IMU/AP_IMU_Oilpan.cpp | 2 +- libraries/AP_IMU/IMU.h | 8 +- 7 files changed, 90 insertions(+), 83 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC.h b/libraries/AP_ADC/AP_ADC.h index cafc460c1a..5d749c9ae9 100644 --- a/libraries/AP_ADC/AP_ADC.h +++ b/libraries/AP_ADC/AP_ADC.h @@ -33,10 +33,11 @@ class AP_ADC 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 + The function returns the amount of time (in microseconds) + since the last call to Ch6(). */ virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0; + private: }; diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index 1625809755..ec9959f407 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -19,9 +19,11 @@ XCK2 = SCK = pin PH2 Chip Select pin is PC4 (33) [PH6 (9)] We are using the 16 clocks per conversion timming to increase efficiency (fast) - The sampling frequency is 400Hz (Timer2 overflow interrupt) + + The sampling frequency is 1kHz (Timer2 overflow interrupt) + So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so - we have an 4x oversampling and averaging. + we have an 10x oversampling and averaging. Methods: Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt) @@ -60,14 +62,14 @@ 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) -{ - /* Wait for empty transmit buffer */ - while ( !( UCSR2A & (1 << UDRE2)) ); - /* Put data into buffer, sends the data */ - UDR2 = data; - /* Wait for data to be received */ + +static uint32_t last_ch6_micros; + +static inline unsigned char ADC_SPI_transfer(unsigned char data) +{ + /* Put data into buffer, sends the data */ + UDR2 = data; + /* Wait for data to be received */ while ( !(UCSR2A & (1 << RXC2)) ); /* Get and return received data from buffer */ return UDR2; @@ -75,38 +77,39 @@ static unsigned char ADC_SPI_transfer(unsigned char data) ISR (TIMER2_OVF_vect) -{ - uint8_t ch; - - //bit_set(PORTL,6); // To test performance - +{ + uint8_t ch; + 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++){ - 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 - - _count[ch]++; - if (_count[ch] == 0) { + + for (ch = 0; ch < 8; ch++) { + uint16_t count = _count[ch]; + uint32_t sum = _sum[ch]; + + if (++count == 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 = 0; + count = 1; + last_ch6_micros = micros(); } - _sum[ch] += adc_tmp; - } - + _count[ch] = count; + + sum += ADC_SPI_transfer(0) << 8; // Read first byte + sum += ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command + + _sum[ch] = sum; + } + bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) - //bit_clear(PORTL,6); // To test performance - TCNT2 = 104; // 400Hz interrupt -} - - + TCNT2 = 200; +} + + // Constructors //////////////////////////////////////////////////////////////// AP_ADC_ADS7844::AP_ADC_ADS7844() { @@ -114,21 +117,21 @@ AP_ADC_ADS7844::AP_ADC_ADS7844() // Public Methods ////////////////////////////////////////////////////////////// void AP_ADC_ADS7844::Init(void) -{ - pinMode(ADC_CHIP_SELECT, OUTPUT); - - digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low) - - // Setup Serial Port2 in SPI mode - UBRR2 = 0; - DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode - // Set MSPI mode of operation and SPI data mode 0. - UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2); - // Enable receiver and transmitter. - UCSR2B = (1 << RXEN2) | (1 << TXEN2); - // Set Baud rate - UBRR2 = 2; // SPI clock running at 2.6MHz - +{ + pinMode(ADC_CHIP_SELECT, OUTPUT); + + digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low) + + // Setup Serial Port2 in SPI mode + UBRR2 = 0; + DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode + // Set MSPI mode of operation and SPI data mode 0. + UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2); + // Enable receiver and transmitter. + UCSR2B = (1 << RXEN2) | (1 << TXEN2); + // 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++) { @@ -139,17 +142,18 @@ void AP_ADC_ADS7844::Init(void) _sum[i] = adc_tmp; } - - // Enable Timer2 Overflow interrupt to capture ADC data - TIMSK2 = 0; // Disable interrupts - TCCR2A = 0; // normal counting mode - TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of 256 - TCNT2 = 0; - TIFR2 = _BV(TOV2); // clear pending interrupts; - TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt -} - -// Read one channel value + last_ch6_micros = micros(); + + // Enable Timer2 Overflow interrupt to capture ADC data + TIMSK2 = 0; // Disable interrupts + TCCR2A = 0; // normal counting mode + TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of 256 + TCNT2 = 0; + TIFR2 = _BV(TOV2); // clear pending interrupts; + TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt +} + +// Read one channel value uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num) { uint16_t count; @@ -174,7 +178,7 @@ uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num) // 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) +uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) { uint16_t count[6]; uint32_t sum[6]; @@ -203,7 +207,9 @@ uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) 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]; -} + // return number of microseconds since last call + uint32_t us = micros(); + uint32_t ret = us - last_ch6_micros; + last_ch6_micros = us; + return ret; +} diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index c4b99ecaed..35f72d36db 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -24,9 +24,9 @@ class AP_ADC_ADS7844 : public AP_ADC uint16_t Ch(unsigned char ch_num); // Read 6 sensors at once - uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result); - - private: -}; + uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); + + private: +}; #endif diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp index 1684b161b7..108bb1120f 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ b/libraries/AP_ADC/AP_ADC_HIL.cpp @@ -47,11 +47,11 @@ void AP_ADC_HIL::Init(void) uint16_t AP_ADC_HIL::Ch(unsigned char ch_num) { return adcValue[ch_num]; -} +} // Read 6 channel values -uint16_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result) -{ +uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result) +{ for (uint8_t i=0; i<6; i++) { result[i] = Ch(channel_numbers[i]); } diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index ed0c039289..1794da8eaf 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -34,12 +34,12 @@ class AP_ADC_HIL : public AP_ADC // Read the sensor, part of public AP_ADC interface uint16_t Ch(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, + uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); + + /// + // Set the adc raw values given the current rotations rates, // temps, accels, and pressures void setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp, int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress); diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp index b332142520..73cef75e4e 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.cpp +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -279,7 +279,7 @@ AP_IMU_Oilpan::update(void) int tc_temp = _adc->Ch(_gyro_temp_ch); uint16_t adc_values[6]; - _ticks = _adc->Ch6(_sensors, adc_values); + _sample_time = _adc->Ch6(_sensors, adc_values); // convert corrected gyro readings to delta acceleration // diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h index 42a128ea58..1b881bc355 100644 --- a/libraries/AP_IMU/IMU.h +++ b/libraries/AP_IMU/IMU.h @@ -81,7 +81,7 @@ public: /// /// @returns number of seconds /// - float get_delta_time(void) { return _ticks * (2.5/1000.0); } + float get_delta_time(void) { return _sample_time * 1.0e-6; } /// A count of bad sensor readings /// @@ -98,9 +98,9 @@ protected: /// Most recent gyro reading obtained by ::update Vector3f _gyro; - /// number of 2.5ms ticks that the accel and gyro values - /// were calculated from - uint16_t _ticks; + /// number of microseconds that the accel and gyro values + /// were sampled over + uint32_t _sample_time; }; #endif