Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
jasonshort 2011-06-12 23:50:15 +00:00
parent b07c32acd0
commit 65345160d1
2 changed files with 83 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -2,22 +2,22 @@
AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega AP_ADC_ADS7844.cpp - ADC ADS7844 Library for Ardupilot Mega
Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
Modified by John Ihlein 6/19/2010 to: Modified by John Ihlein 6 / 19 / 2010 to:
1)Prevent overflow of adc_counter when more than 8 samples collected between reads. Probably 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. only an issue on initial read of ADC at program start.
2)Reorder analog read order as follows: 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 This library is free software; you can redistribute it and / or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. 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) External ADC ADS7844 is connected via Serial port 2 (in SPI mode)
TXD2 = MOSI = pin PH1 TXD2 = MOSI = pin PH1
RXD2 = MISO = pin PH0 RXD2 = MISO = pin PH0
XCK2 = SCK = pin PH2 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) 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 400Hz (Timer2 overflow interrupt)
So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
@ -34,7 +34,7 @@
Chennel 0 : yaw rate, r Chennel 0 : yaw rate, r
Channel 1 : roll rate, p Channel 1 : roll rate, p
Channel 2 : pitch rate, q Channel 2 : pitch rate, q
Channel 3 : x/y gyro temperature Channel 3 : x / y gyro temperature
Channel 4 : x acceleration, aX Channel 4 : x acceleration, aX
Channel 5 : y acceleration, aY Channel 5 : y acceleration, aY
Channel 6 : z acceleration, aZ Channel 6 : z acceleration, aZ
@ -42,28 +42,28 @@
*/ */
extern "C" { extern "C" {
// AVR LibC Includes // AVR LibC Includes
#include <inttypes.h> #include <inttypes.h>
#include <avr/interrupt.h> #include <avr/interrupt.h>
#include "WConstants.h" #include "WConstants.h"
} }
#include "AP_ADC_ADS7844.h" #include "AP_ADC_ADS7844.h"
// Commands for reading ADC channels on ADS7844 // Commands for reading ADC channels on ADS7844
static const unsigned char adc_cmd[9]= { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 }; 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 uint16_t _filter[8][ADC_FILTER_SIZE];
static volatile unsigned char adc_counter[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; static volatile uint8_t _filter_index;
static unsigned char ADC_SPI_transfer(unsigned char data) static unsigned char ADC_SPI_transfer(unsigned char data)
{ {
/* Wait for empty transmit buffer */ /* Wait for empty transmit buffer */
while ( !( UCSR2A & (1<<UDRE2)) ); while ( !( UCSR2A & (1 << UDRE2)) );
/* Put data into buffer, sends the data */ /* Put data into buffer, sends the data */
UDR2 = data; UDR2 = data;
/* Wait for data to be received */ /* Wait for data to be received */
while ( !(UCSR2A & (1<<RXC2)) ); while ( !(UCSR2A & (1 << RXC2)) );
/* Get and return received data from buffer */ /* Get and return received data from buffer */
return UDR2; return UDR2;
} }
@ -71,27 +71,33 @@ static unsigned char ADC_SPI_transfer(unsigned char data)
ISR (TIMER2_OVF_vect) ISR (TIMER2_OVF_vect)
{ {
uint8_t ch; uint8_t ch;
unsigned int adc_tmp; 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)
for (ch=0;ch<8;ch++) ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
{
if (adc_counter[ch] >= 16) // To prevent overflow of adc_value for (ch = 0; ch < 8; ch++){
{ // adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte
adc_value[ch] /= 2; adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command
adc_counter[ch] /= 2;
} // Fill our Moving average filter
adc_tmp = ADC_SPI_transfer(0)<<8; // Read first byte _filter[ch][_filter_index] = adc_tmp >> 3;
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 // increment our filter
} _filter_index++;
bit_set(PORTC,4); // Disable Chip Select (PIN PC4)
//bit_clear(PORTL,6); // To test performance // loop our filter
TCNT2 = 104; // 400 Hz 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 ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void AP_ADC_ADS7844::Init(void) 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 // Setup Serial Port2 in SPI mode
UBRR2 = 0; UBRR2 = 0;
DDRH |= (1<<PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode
// Set MSPI mode of operation and SPI data mode 0. // Set MSPI mode of operation and SPI data mode 0.
UCSR2C = (1<<UMSEL21)|(1<<UMSEL20); //|(0<<UCPHA2)|(0<<UCPOL2); UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2);
// Enable receiver and transmitter. // Enable receiver and transmitter.
UCSR2B = (1<<RXEN2)|(1<<TXEN2); UCSR2B = (1 << RXEN2) | (1 << TXEN2);
// Set Baud rate // Set Baud rate
UBRR2 = 2; // SPI clock running at 2.6MHz UBRR2 = 2; // SPI clock running at 2.6MHz
// Enable Timer2 Overflow interrupt to capture ADC data // Enable Timer2 Overflow interrupt to capture ADC data
TIMSK2 = 0; // Disable interrupts TIMSK2 = 0; // Disable interrupts
TCCR2A = 0; // normal counting mode TCCR2A = 0; // normal counting mode
TCCR2B = _BV(CS21)|_BV(CS22); // Set prescaler of 256 TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of 256
TCNT2 = 0; TCNT2 = 0;
TIFR2 = _BV(TOV2); // clear pending interrupts; TIFR2 = _BV(TOV2); // clear pending interrupts;
TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt
} }
// Read one channel value // Read one channel value
int AP_ADC_ADS7844::Ch(unsigned char ch_num) int AP_ADC_ADS7844::Ch(unsigned char ch_num)
{ {
int result; uint16_t result = 0;
while(adc_counter[ch_num] < 2) { } // Wait for at least 2 samples in accumlator //while(adc_counter[ch_num] < 2) { } // Wait for at least 2 samples in accumlator
cli(); // stop interrupts
if (adc_counter[ch_num]>0) cli();
result = adc_value[ch_num]/adc_counter[ch_num];
else // sum our filter
result = 0; for(uint8_t i = 0; i < ADC_FILTER_SIZE; i++){
result += _filter[ch_num][i];
adc_value[ch_num] = 0; // Initialize for next reading }
adc_counter[ch_num] = 0;
sei(); // resume interrupts
return(result); sei();
// average our sampels
result /= ADC_FILTER_SIZE;
return(result);
} }

11
libraries/AP_ADC/AP_ADC_ADS7844.h
View File

@ -9,16 +9,19 @@
#define ADC_DATAIN 50 // MISO #define ADC_DATAIN 50 // MISO
#define ADC_SPICLOCK 52 // SCK #define ADC_SPICLOCK 52 // SCK
#define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40 #define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40
#define ADC_FILTER_SIZE 6
#include "AP_ADC.h" #include "AP_ADC.h"
#include <inttypes.h>
class AP_ADC_ADS7844 : public AP_ADC class AP_ADC_ADS7844 : public AP_ADC
{ {
public: public:
AP_ADC_ADS7844(); // Constructor AP_ADC_ADS7844(); // Constructor
void Init(); void Init();
int Ch(unsigned char ch_num); int Ch(unsigned char ch_num);
private:
private:
}; };
#endif #endif