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

Minimized the accel_filtered vector to be faster and more accurate.

Browse Source 
Made the Filter include the Accels and moved from 8 to 4 terms in the filter
This commit is contained in:
Jason Short 2011-09-30 18:40:29 -07:00
parent 0064e80e4e
commit 9a4bd31a8b
5 changed files with 21 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Attitude.pde
View File

@ -1,6 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// XXX TODO: convert these PI rate controlers to a Class
static int static int
get_stabilize_roll(long target_angle) get_stabilize_roll(long target_angle)
{ {

5
ArduCopter/test.pde
View File

@ -430,9 +430,6 @@ test_imu(uint8_t argc, const Menu::arg *argv)
while(1){ while(1){
//delay(20); //delay(20);
if (millis() - fast_loopTimer >= 5) { if (millis() - fast_loopTimer >= 5) {
//delta_ms_fast_loop = millis() - fast_loopTimer;
//G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
//fast_loopTimer = millis();
// IMU // IMU
// --- // ---
@ -451,7 +448,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
} }
if(medium_loopCounter == 20){ if(medium_loopCounter == 20){
read_radio(); //read_radio();
medium_loopCounter = 0; medium_loopCounter = 0;
//tuning(); //tuning();
//dcm.kp_roll_pitch((float)g.rc_6.control_in / 2000.0); //dcm.kp_roll_pitch((float)g.rc_6.control_in / 2000.0);

16
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -230,26 +230,34 @@ uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result)
if(filter_result){ if(filter_result){
uint32_t _filter_sum; uint32_t _filter_sum;
for (i = 0; i < 3; i++) {
for (i = 0; i < 6; i++) {
// move most recent result into filter // move most recent result into filter
_filter[i][_filter_index] = result[i]; _filter[i][_filter_index] = result[i];
_filter_sum = 0; _filter_sum = 0;
// sum the filter // sum the filter
for (uint8_t n = 0; n < 8; n++) { for (uint8_t n = 0; n < ADC_FILTER_SIZE; n++) {
_filter_sum += _filter[i][n]; _filter_sum += _filter[i][n];
} }
result[i] = _filter_sum / 8; // filter does a moving average on last 4 reads, sums half with
// half of last filtered value
result[i] = (_filter_sum >> 3) + (_prev[i] >> 1); // divide by 8, divide by 2
// save old result
_prev[i] = result[i];
} }
// increment filter index // increment filter index
_filter_index++; _filter_index++;
// loop our filter // loop our filter
if(_filter_index == 8) if(_filter_index == ADC_FILTER_SIZE)
_filter_index = 0; _filter_index = 0;
} }
// return number of microseconds since last call // return number of microseconds since last call
uint32_t us = micros(); uint32_t us = micros();
uint32_t ret = us - last_ch6_micros; uint32_t ret = us - last_ch6_micros;

7
libraries/AP_ADC/AP_ADC_ADS7844.h
View File

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

6
libraries/AP_IMU/AP_IMU_Oilpan.cpp
View File

@ -293,9 +293,9 @@ AP_IMU_Oilpan::update(void)
_accel.y = _accel_scale * _sensor_in(4, adc_values[4], tc_temp); _accel.y = _accel_scale * _sensor_in(4, adc_values[4], tc_temp);
_accel.z = _accel_scale * _sensor_in(5, adc_values[5], tc_temp); _accel.z = _accel_scale * _sensor_in(5, adc_values[5], tc_temp);
_accel_filtered.x = _accel_filtered.x * .98 + _accel.x * .02; _accel_filtered.x = _accel_filtered.x / 2 + _accel.x / 2;
_accel_filtered.y = _accel_filtered.y * .98 + _accel.y * .02; _accel_filtered.y = _accel_filtered.y / 2 + _accel.y / 2;
_accel_filtered.z = _accel_filtered.z * .98 + _accel.z * .02; _accel_filtered.z = _accel_filtered.z / 2 + _accel.z / 2;
// always updated // always updated
return true; return true;