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

Full Filter Rework FIR replaced by BiQuad

Browse source
This commit is contained in:
borisbstyle 2016-01-12 01:47:09 +01:00
parent 7ef593c54f
commit 56098a63ca
6 changed files with 96 additions and 61 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -24,6 +24,8 @@
#include "common/filter.h"
#include "common/maths.h"
#include "drivers/gyro_sync.h"
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
@ -38,49 +40,64 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
return filter->state;
}
static int8_t gyroFIRCoeff_500[FILTER_TAPS] = { 0, 18, 14, 16, 20, 22, 24, 25, 25, 24, 20, 18, 12, 18 }; // looptime=500;
static int8_t gyroFIRCoeff_1000[FILTER_TAPS] = { 0, 0, 0, 0, 0, 0, 0, 12, 23, 40, 51, 52, 40, 38 }; // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
void setBiQuadCoefficients(int type, biquad_t *state) {
static int8_t deltaFIRCoeff_500[FILTER_TAPS] = {36, 12, 14, 14, 16, 16, 18, 18, 18, 16, 16, 14, 12, 36};
static int8_t deltaFIRCoeff_1000[FILTER_TAPS] = {0, 0, 0, 0, 0, 18, 12, 28, 40, 44, 40, 32, 22, 20};
/* zero initial samples */
state->x1=0;
state->x2=0;
state->y1=0;
state->y2=0;
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_type, uint32_t targetLooptime)
{
int8_t *filterCoeff;
switch(filter_type){
case(0):
filterCoeff = NULL;
break;
case(1):
/* set coefficients */
switch(type) {
case(GYRO_FILTER):
if (targetLooptime == 500) {
filterCoeff = gyroFIRCoeff_500;
} else { // filter for 1kHz looptime
filterCoeff = gyroFIRCoeff_1000;
state->a0= 0.007820199;
state->a1= 0.015640399;
state->a2= 0.007820199;
state->a3= -1.73472382;
state->a4= 0.766004619;
} else {
state->a0= 0.027859711;
state->a1= 0.055719422;
state->a2= 0.027859711;
state->a3= -1.47547752;
state->a4= 0.586916365;
}
break;
case(2):
case(DELTA_FILTER):
if (targetLooptime == 500) {
filterCoeff = deltaFIRCoeff_500;
} else { // filter for 1kHz looptime
filterCoeff = deltaFIRCoeff_1000;
state->a0= 0.003621679;
state->a1= 0.007243357;
state->a2= 0.003621679;
state->a3= -1.82269350;
state->a4= 0.837180216;
} else {
state->a0= 0.013359181;
state->a1= 0.026718362;
state->a2= 0.013359181;
state->a3= -1.64745762;
state->a4= 0.700894342;
}
}
return filterCoeff;
}
// Thanks to Qcopter & BorisB & DigitalEntity
void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS])
/* Computes a BiQuad filter on a sample */
float applyBiQuadFilter(float sample, biquad_t *state)
{
int32_t FIRsum;
FIRsum = 0;
int i;
float result;
for (i = 0; i <= FILTER_TAPS-2; i++) {
state[i] = state[i + 1];
FIRsum += state[i] * (int16_t)coeff[i];
}
state[FILTER_TAPS-1] = *data;
FIRsum += state[FILTER_TAPS-1] * coeff[FILTER_TAPS-1];
*data = FIRsum / 256;
/* compute result */
result = state->a0 * sample + state->a1 * state->x1 + state->a2 * state->x2 -
state->a3 * state->y1 - state->a4 * state->y2;
/* shift x1 to x2, sample to x1 */
state->x2 = state->x1;
state->x1 = sample;
/* shift y1 to y2, result to y1 */
state->y2 = state->y1;
state->y1 = result;
return result;
}

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

@ -23,6 +23,18 @@ typedef struct filterStatePt1_s {
float constdT;
} filterStatePt1_t;
/* this holds the data required to update samples thru a filter */
typedef struct biquad_s {
float a0, a1, a2, a3, a4;
float x1, x2, y1, y2;
} biquad_t;
typedef enum {
GYRO_FILTER,
ACCELEROMETER_FILTER,
DELTA_FILTER
} filterType_e;
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime);
void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS]);
float applyBiQuadFilter(float sample, biquad_t * b);
void setBiQuadCoefficients(int type, biquad_t *state);

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

@ -747,8 +747,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf, targetLooptime));
useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

31
src/main/flight/pid.c
View file

@ -112,8 +112,10 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static airModePlus_t airModePlusAxisState[3];
static int8_t * deltaFIRTable = 0L;
static int16_t deltaFIRState[3][FILTER_TAPS];
//static int8_t * deltaFIRTable = 0L;
//static int16_t deltaFIRState[3][FILTER_TAPS];
static bool deltaStateIsSet = false;
static biquad_t deltaBiQuadState[3];
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@ -126,7 +128,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
if (!deltaStateIsSet) {
for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(DELTA_FILTER, &deltaBiQuadState[axis]);
deltaStateIsSet = true;
}
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -210,16 +215,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
delta = RateError - lastError[axis];
lastError[axis] = RateError;
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT);
/* Delta filtering is in integers, which should be fine */
int16_t deltaTemp = (int16_t) delta;
filterApplyFIR(&deltaTemp, deltaFIRState[axis], deltaFIRTable);
delta = (float) deltaTemp;
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output
@ -249,15 +250,17 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
UNUSED(rxConfig);
int axis;
int16_t delta;
int32_t PTerm, ITerm, DTerm;
int32_t PTerm, ITerm, DTerm, delta;
static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100;
if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
if (!deltaStateIsSet) {
for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(DELTA_FILTER, &deltaBiQuadState[axis]);
deltaStateIsSet = true;
}
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -340,15 +343,15 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
}
//-----calculate D-term
delta = (int16_t) RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis]));
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
filterApplyFIR(&delta, deltaFIRState[axis], deltaFIRTable);
DTerm = (delta * 3 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output

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

@ -38,17 +38,20 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static int8_t * gyroFIRTable = 0L;
static int16_t gyroFIRState[3][FILTER_TAPS];
static biquad_t gyroBiQuadState[3];
static bool useSoftFilter;
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t useFilter)
{
int axis;
gyroConfig = gyroConfigToUse;
gyroFIRTable = filterTableToUse;
if (useFilter) {
useSoftFilter = true;
for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(GYRO_FILTER, &gyroBiQuadState[axis]);
}
}
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@ -126,9 +129,10 @@ void gyroUpdate(void)
return;
}
if (gyroFIRTable) {
if (useSoftFilter) {
int axis;
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) filterApplyFIR(&gyroADC[axis], gyroFIRState[axis], gyroFIRTable);
//for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) filterApplyFIR(&gyroADC[axis], gyroFIRState[axis], gyroFIRTable);
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]);
}
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.
} gyroConfig_t;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse);
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t useFilter);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);