mirror of https://github.com/ArduPilot/ardupilot
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:
parent
b07c32acd0
commit
65345160d1
|
@ -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
|
|
||||||
result = 0;
|
|
||||||
|
|
||||||
adc_value[ch_num] = 0; // Initialize for next reading
|
// sum our filter
|
||||||
adc_counter[ch_num] = 0;
|
for(uint8_t i = 0; i < ADC_FILTER_SIZE; i++){
|
||||||
sei();
|
result += _filter[ch_num][i];
|
||||||
return(result);
|
}
|
||||||
|
|
||||||
|
// resume interrupts
|
||||||
|
sei();
|
||||||
|
|
||||||
|
// average our sampels
|
||||||
|
result /= ADC_FILTER_SIZE;
|
||||||
|
|
||||||
|
return(result);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue