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

Heavy Accel Filter + mild Gyro Filter. To use, filter_result must be set to true on ADC. Off by default.

This commit is contained in:
Jason Short 2011-10-01 15:44:53 -07:00
parent cf59e8ad82
commit 945a7c13a7
2 changed files with 31 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -128,7 +128,7 @@ ISR (TIMER2_OVF_vect)
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_ADC_ADS7844::AP_ADC_ADS7844() : AP_ADC_ADS7844::AP_ADC_ADS7844() :
_filter_index(0), _filter_index_accel(0),
filter_result(false) filter_result(false)
{ {
} }
@ -161,7 +161,6 @@ void AP_ADC_ADS7844::Init(void)
} }
last_ch6_micros = micros(); last_ch6_micros = micros();
_filter_index = 0;
// Enable Timer2 Overflow interrupt to capture ADC data // Enable Timer2 Overflow interrupt to capture ADC data
TIMSK2 = 0; // Disable interrupts TIMSK2 = 0; // Disable interrupts
@ -226,35 +225,44 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
result[i] = sum[i] / count[i]; result[i] = sum[i] / count[i];
} }
// filter ch 0,1,2 for smoother Gyro output.
if(filter_result){ if(filter_result){
uint32_t _filter_sum; uint32_t _sum_accel;
for (i = 0; i < 6; i++) { // simple Gyro Filter
for (i = 0; i < 3; i++) {
// add prev filtered value to new raw value, divide by 2
result[i] = (_prev_gyro[i] + result[i]) >> 1;
// remember the filtered value
_prev_gyro[i] = result[i];
}
// Accel filter
for (i = 0; i < 3; i++) {
// move most recent result into filter // move most recent result into filter
_filter[i][_filter_index] = result[i]; _filter_accel[i][_filter_index_accel] = result[i+3];
// clear the sum
_sum_accel = 0;
_filter_sum = 0;
// sum the filter // sum the filter
for (uint8_t n = 0; n < ADC_FILTER_SIZE; n++) { for (uint8_t n = 0; n < ADC_ACCEL_FILTER_SIZE; n++) {
_filter_sum += _filter[i][n]; _sum_accel += _filter_accel[i][n];
} }
// filter does a moving average on last 4 reads, sums half with // filter does a moving average on last 8 reads, sums half with half of last filtered value
// half of last filtered value
result[i] = (_filter_sum >> 3) + (_prev[i] >> 1); // divide by 8, divide by 2
// save old result // save old result
_prev[i] = result[i]; _prev_accel[i] = result[i+3] = (_sum_accel >> 4) + (_prev_accel[i] >> 1); // divide by 16, divide by 2
} }
// increment filter index // increment filter index
_filter_index++; _filter_index_accel++;
// loop our filter // loop our filter
if(_filter_index == ADC_FILTER_SIZE) if(_filter_index_accel == ADC_ACCEL_FILTER_SIZE)
_filter_index = 0; _filter_index_accel = 0;
} }

11
libraries/AP_ADC/AP_ADC_ADS7844.h
View File

@ -10,8 +10,8 @@
#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
// DO NOT CHANGE FROM 4 // DO NOT CHANGE FROM 8!!
#define ADC_FILTER_SIZE 4 #define ADC_ACCEL_FILTER_SIZE 8
#include "AP_ADC.h" #include "AP_ADC.h"
#include <inttypes.h> #include <inttypes.h>
@ -30,9 +30,10 @@ class AP_ADC_ADS7844 : public AP_ADC
bool filter_result; bool filter_result;
private: private:
uint16_t _filter[6][ADC_FILTER_SIZE]; uint16_t _filter_accel[3][ADC_ACCEL_FILTER_SIZE];
uint16_t _prev[6]; uint16_t _prev_gyro[3];
uint8_t _filter_index; uint16_t _prev_accel[3];
uint8_t _filter_index_accel;
}; };