From 65345160d1068442c7e35e66125cec53695b4237 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 12 Jun 2011 23:50:15 +0000 Subject: [PATCH] Implemented moving average filter to deal with noise issues on quads, default is a 6 member filter. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2551 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_ADC/AP_ADC_ADS7844.cpp | 141 +++++++++++++++------------- libraries/AP_ADC/AP_ADC_ADS7844.h | 11 ++- 2 files changed, 83 insertions(+), 69 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index 723181dfc8..1cfb31829d 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -2,22 +2,22 @@ AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega Code by Jordi Mu�oz and Jose Julio. DIYDrones.com - Modified by John Ihlein 6/19/2010 to: - 1)Prevent overflow of adc_counter when more than 8 samples collected between reads. Probably - only an issue on initial read of ADC at program start. + Modified by John Ihlein 6 / 19 / 2010 to: + 1)Prevent overflow of adc_counter when more than 8 samples collected between reads. Probably + only an issue on initial read of ADC at program start. 2)Reorder analog read order as follows: - p, q, r, ax, ay, az + p, q, r, ax, ay, az - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. External ADC ADS7844 is connected via Serial port 2 (in SPI mode) TXD2 = MOSI = pin PH1 RXD2 = MISO = pin PH0 XCK2 = SCK = pin PH2 - Chip Select pin is PC4 (33) [PH6 (9)] + 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) So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so @@ -34,7 +34,7 @@ Chennel 0 : yaw rate, r Channel 1 : roll rate, p Channel 2 : pitch rate, q - Channel 3 : x/y gyro temperature + Channel 3 : x / y gyro temperature Channel 4 : x acceleration, aX Channel 5 : y acceleration, aY Channel 6 : z acceleration, aZ @@ -42,28 +42,28 @@ */ extern "C" { - // AVR LibC Includes - #include - #include - #include "WConstants.h" + // AVR LibC Includes + #include + #include + #include "WConstants.h" } #include "AP_ADC_ADS7844.h" // 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 unsigned int adc_value[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; -static volatile unsigned char adc_counter[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; +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; static unsigned char ADC_SPI_transfer(unsigned char data) { /* Wait for empty transmit buffer */ - while ( !( UCSR2A & (1<= 16) // To prevent overflow of adc_value - { // - adc_value[ch] /= 2; - adc_counter[ch] /= 2; - } - 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 - adc_value[ch] += adc_tmp>>3; // Shift to 12 bits - adc_counter[ch]++; // Number of samples - } - bit_set(PORTC,4); // Disable Chip Select (PIN PC4) - //bit_clear(PORTL,6); // To test performance - TCNT2 = 104; // 400 Hz + //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 + + for (ch = 0; ch < 8; ch++){ + 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; + } + + // 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 } @@ -103,45 +109,50 @@ AP_ADC_ADS7844::AP_ADC_ADS7844() // Public Methods ////////////////////////////////////////////////////////////// void AP_ADC_ADS7844::Init(void) { - pinMode(ADC_CHIP_SELECT,OUTPUT); + pinMode(ADC_CHIP_SELECT, OUTPUT); - digitalWrite(ADC_CHIP_SELECT,HIGH); // Disable device (Chip select is active low) + digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low) // Setup Serial Port2 in SPI mode - UBRR2 = 0; - DDRH |= (1<0) - result = adc_value[ch_num]/adc_counter[ch_num]; - else - result = 0; - - adc_value[ch_num] = 0; // Initialize for next reading - adc_counter[ch_num] = 0; - sei(); - return(result); + // stop interrupts + cli(); + + // sum our filter + for(uint8_t i = 0; i < ADC_FILTER_SIZE; i++){ + result += _filter[ch_num][i]; + } + + // resume interrupts + sei(); + + // average our sampels + result /= ADC_FILTER_SIZE; + + return(result); } diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index 55014603bc..6d1f518be0 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -9,16 +9,19 @@ #define ADC_DATAIN 50 // MISO #define ADC_SPICLOCK 52 // SCK #define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40 +#define ADC_FILTER_SIZE 6 #include "AP_ADC.h" +#include class AP_ADC_ADS7844 : public AP_ADC { - public: + public: AP_ADC_ADS7844(); // Constructor - void Init(); - int Ch(unsigned char ch_num); - private: + void Init(); + int Ch(unsigned char ch_num); + + private: }; #endif