Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00
Code Issues Wiki Activity

Simplify filters

Browse source
This commit is contained in:
borisbstyle 2015-12-10 21:37:57 +01:00
parent 6f4d741cd0
commit 8f0d7de791
5 changed files with 20 additions and 35 deletions
Download patch file Download diff file Expand all files Collapse all files

34
src/main/common/filter.c
View file

@ -10,10 +10,8 @@
#include <stdlib.h> #include <stdlib.h>
#include <math.h> #include <math.h>
#include "common/axis.h"
#include "common/filter.h" #include "common/filter.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h"
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime) // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
@ -29,33 +27,23 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
return filter->state; return filter->state;
} }
/** // 7 Tap FIR filter as described here:
* Typical quadcopter motor noise frequency (at 50% throttle): // Thanks to Qcopter
* 450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz void filterApply7TapFIR(int16_t data[]) {
* 250-sized, 2300kv, 5x4.5 props, 4S : 14139rpm = 235Hz int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 }; // TODO - More coefficients needed. Now fixed to 1khz
*/ static int16_t gyro_delay[3][7] = { {0}, {0}, {0} };
static int8_t gyroFIRCoeff_1000[7] = { 12, 23, 40, 51, 52, 40, 38 }; // 1khz; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132hz
int8_t * filterGetFIRCoefficientsTable(void)
{
return gyroFIRCoeff_1000;
}
// 9 Tap FIR filter as described here:
// Thanks to Qcopter & BorisB & DigitalEntity
void filterApplyFIR(int16_t data[3], int16_t state[3][7], int8_t coeff[7])
{
int32_t FIRsum; int32_t FIRsum;
int axis, i; int axis, i;
// 7 tap FIR, <-20dB at >170Hz with looptime 1ms, groupdelay = 2.5ms
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
FIRsum = 0; FIRsum = 0;
for (i = 0; i <= 7; i++) { for (i = 0; i <= 5; i++) {
state[axis][i] = state[axis][i + 1]; gyro_delay[axis][i] = gyro_delay[axis][i + 1];
FIRsum += state[axis][i] * (int16_t)coeff[i]; FIRsum += gyro_delay[axis][i] * FIRcoeff[i];
} }
state[axis][8] = data[axis]; gyro_delay[axis][6] = data[axis];
FIRsum += state[axis][8] * coeff[8]; FIRsum += gyro_delay[axis][6] * FIRcoeff[6];
data[axis] = FIRsum / 256; data[axis] = FIRsum / 256;
} }
} }

3
src/main/common/filter.h
View file

@ -13,5 +13,4 @@ typedef struct filterStatePt1_s {
} filterStatePt1_t; } filterStatePt1_t;
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt); float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
int8_t * filterGetFIRCoefficientsTable(void); void filterApplyFIR(int16_t data[3]);
void filterApplyFIR(int16_t data[3], int16_t state[3][7], int8_t coeff[7]);

5
src/main/config/config.c
View file

@ -694,9 +694,8 @@ void activateConfig(void)
&currentProfile->pidProfile &currentProfile->pidProfile
); );
if (currentProfile->pidProfile.gyro_soft_lpf) {
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable()); // Leave this for more coefficients in the future useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf); // Leave this for more coefficients in the future
}
#ifdef TELEMETRY #ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig); telemetryUseConfig(&masterConfig.telemetryConfig);

11
src/main/sensors/gyro.c
View file

@ -38,16 +38,15 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig; static gyroConfig_t *gyroConfig;
static int8_t * gyroFIRTable = 0L; static uint8_t gyroFIRFilter;
static int16_t gyroFIRState[3][7];
gyro_t gyro; // gyro access functions gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0; sensor_align_e gyroAlign = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse) void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t filter)
{ {
gyroConfig = gyroConfigToUse; gyroConfig = gyroConfigToUse;
gyroFIRTable = filterTableToUse; gyroFIRFilter = filter;
} }
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired) void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@ -125,8 +124,8 @@ void gyroUpdate(void)
return; return;
} }
if (gyroFIRTable) { if (gyroFIRFilter) {
filterApplyFIR(gyroADC, gyroFIRState, gyroFIRTable); filterApplyFIR(gyroADC);
} }
alignSensors(gyroADC, gyroADC, gyroAlign); alignSensors(gyroADC, gyroADC, gyroAlign);

2
src/main/sensors/gyro.h
View file

@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t; } gyroConfig_t;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse); void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t filter);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void); void gyroUpdate(void);
bool isGyroCalibrationComplete(void); bool isGyroCalibrationComplete(void);