Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 10:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Implemented moving average filter to deal with noise issues on quads, default is a 6 member filter.

Browse Source 
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

51
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -53,8 +53,8 @@ extern "C" {
// 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)
{ {
@ -72,23 +72,29 @@ 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) bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4)
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
for (ch=0;ch<8;ch++)
{ for (ch = 0; ch < 8; ch++){
if (adc_counter[ch] >= 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(0) << 8; // Read first byte
adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command 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 // 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_set(PORTC, 4); // Disable Chip Select (PIN PC4)
//bit_clear(PORTL,6); // To test performance //bit_clear(PORTL,6); // To test performance
TCNT2 = 104; // 400 Hz TCNT2 = 104; // 400 Hz
@ -130,18 +136,23 @@ void AP_ADC_ADS7844::Init(void)
// 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
// stop interrupts
cli(); cli();
if (adc_counter[ch_num]>0)
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++){
result += _filter[ch_num][i];
}
// resume interrupts
sei(); sei();
// average our sampels
result /= ADC_FILTER_SIZE;
return(result); return(result);
} }

3
libraries/AP_ADC/AP_ADC_ADS7844.h
View File

@ -9,8 +9,10 @@
#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
{ {
@ -18,6 +20,7 @@ class AP_ADC_ADS7844 : public AP_ADC
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:
}; };