rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

BiQuad Cleanup Code

This commit is contained in:
borisbstyle 2016-01-14 16:18:35 +01:00
parent 85af28fd86
commit 1dddd65e67
3 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/main/common/filter.c
View File

@ -53,12 +53,12 @@ biquad_t *BiQuadNewLpf(uint8_t filterCutFreq)
float samplingRate;
samplingRate = 1 / (targetLooptime * 0.000001f);
biquad_t *b;
biquad_t *newState;
float omega, sn, cs, alpha;
float a0, a1, a2, b0, b1, b2;
b = malloc(sizeof(biquad_t));
if (b == NULL)
newState = malloc(sizeof(biquad_t));
if (newState == NULL)
return NULL;
/* setup variables */
@ -75,17 +75,17 @@ biquad_t *BiQuadNewLpf(uint8_t filterCutFreq)
a2 = 1 - alpha;
/* precompute the coefficients */
b->a0 = b0 /a0;
b->a1 = b1 /a0;
b->a2 = b2 /a0;
b->a3 = a1 /a0;
b->a4 = a2 /a0;
newState->a0 = b0 /a0;
newState->a1 = b1 /a0;
newState->a2 = b2 /a0;
newState->a3 = a1 /a0;
newState->a4 = a2 /a0;
/* zero initial samples */
b->x1 = b->x2 = 0;
b->y1 = b->y2 = 0;
newState->x1 = newState->x2 = 0;
newState->y1 = newState->y2 = 0;
return b;
return newState;
}
/* Computes a biquad_t filter on a sample */

10
src/main/flight/pid.c
View File

@ -113,7 +113,7 @@ 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 biquad_t deltaBiQuadState[3];
static biquad_t *deltaBiQuadState[3];
static bool deltaStateIsSet;
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
@ -128,7 +128,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
deltaStateIsSet = true;
}
@ -215,7 +215,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
lastError[axis] = RateError;
if (deltaStateIsSet) {
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
delta = applyBiQuadFilter(delta, deltaBiQuadState[axis]);
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
@ -259,7 +259,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int8_t horizonLevelStrength = 100;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
deltaStateIsSet = true;
}
@ -348,7 +348,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
lastError[axis] = RateError;
if (deltaStateIsSet) {
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis]));
delta = lrintf(applyBiQuadFilter((float) delta, deltaBiQuadState[axis]));
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference

6
src/main/sensors/gyro.c
View File

@ -41,7 +41,7 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static biquad_t gyroBiQuadState[3];
static biquad_t *gyroBiQuadState[3];
static bool gyroFilterStateIsSet;
static uint8_t gyroLpfCutFreq;
int axis;
@ -57,7 +57,7 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz)
void initGyroFilterCoefficients(void) {
if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */
for (axis = 0; axis < 3; axis++) gyroBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(gyroLpfCutFreq);
for (axis = 0; axis < 3; axis++) gyroBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(gyroLpfCutFreq);
gyroFilterStateIsSet = true;
}
}
@ -143,7 +143,7 @@ void gyroUpdate(void)
if (!gyroFilterStateIsSet) {
initGyroFilterCoefficients();
} else {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]));
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], gyroBiQuadState[axis]));
}
}