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; float samplingRate;
samplingRate = 1 / (targetLooptime * 0.000001f); samplingRate = 1 / (targetLooptime * 0.000001f);
biquad_t *b; biquad_t *newState;
float omega, sn, cs, alpha; float omega, sn, cs, alpha;
float a0, a1, a2, b0, b1, b2; float a0, a1, a2, b0, b1, b2;
b = malloc(sizeof(biquad_t)); newState = malloc(sizeof(biquad_t));
if (b == NULL) if (newState == NULL)
return NULL; return NULL;
/* setup variables */ /* setup variables */
@ -75,17 +75,17 @@ biquad_t *BiQuadNewLpf(uint8_t filterCutFreq)
a2 = 1 - alpha; a2 = 1 - alpha;
/* precompute the coefficients */ /* precompute the coefficients */
b->a0 = b0 /a0; newState->a0 = b0 /a0;
b->a1 = b1 /a0; newState->a1 = b1 /a0;
b->a2 = b2 /a0; newState->a2 = b2 /a0;
b->a3 = a1 /a0; newState->a3 = a1 /a0;
b->a4 = a2 /a0; newState->a4 = a2 /a0;
/* zero initial samples */ /* zero initial samples */
b->x1 = b->x2 = 0; newState->x1 = newState->x2 = 0;
b->y1 = b->y2 = 0; newState->y1 = newState->y2 = 0;
return b; return newState;
} }
/* Computes a biquad_t filter on a sample */ /* 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 }; const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static airModePlus_t airModePlusAxisState[3]; static airModePlus_t airModePlusAxisState[3];
static biquad_t deltaBiQuadState[3]; static biquad_t *deltaBiQuadState[3];
static bool deltaStateIsSet; static bool deltaStateIsSet;
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, 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 }; static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { 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; deltaStateIsSet = true;
} }
@ -215,7 +215,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
lastError[axis] = RateError; lastError[axis] = RateError;
if (deltaStateIsSet) { 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 // 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; int8_t horizonLevelStrength = 100;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { 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; deltaStateIsSet = true;
} }
@ -348,7 +348,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
lastError[axis] = RateError; lastError[axis] = RateError;
if (deltaStateIsSet) { 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 // 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 }; int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig; static gyroConfig_t *gyroConfig;
static biquad_t gyroBiQuadState[3]; static biquad_t *gyroBiQuadState[3];
static bool gyroFilterStateIsSet; static bool gyroFilterStateIsSet;
static uint8_t gyroLpfCutFreq; static uint8_t gyroLpfCutFreq;
int axis; int axis;
@ -57,7 +57,7 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz)
void initGyroFilterCoefficients(void) { void initGyroFilterCoefficients(void) {
if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ 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; gyroFilterStateIsSet = true;
} }
} }
@ -143,7 +143,7 @@ void gyroUpdate(void)
if (!gyroFilterStateIsSet) { if (!gyroFilterStateIsSet) {
initGyroFilterCoefficients(); initGyroFilterCoefficients();
} else { } 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]));
} }
} }