Skip to content

Commit

Permalink
BiQuad Cleanup Code
Browse files Browse the repository at this point in the history
  • Loading branch information
@borisbstyle
borisbstyle committed Jan 14, 2016
1 parent 85af28f commit 1dddd65
Show file tree
Hide file tree
Showing 3 changed files with 19 additions and 19 deletions.
22 changes: 11 additions & 11 deletions src/main/common/filter.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));

This comment has been minimized.

Sign in to view

Copy link
@digitalentity

digitalentity Jan 15, 2016

Contributor

I can't remember having malloc in CF. When did it happen?

This comment has been minimized.

Sign in to view

Copy link
@borisbstyle

borisbstyle Jan 15, 2016

Author Member

It is there of course, but what I hear from ledvinap and dominic not part of the coding style to keep it safe.
Will remove it when I make a pr to cf
if (newState == NULL)
return NULL;

/* setup variables */
Expand All @@ -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 */
Expand Down
10 changes: 5 additions & 5 deletions src/main/flight/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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;
}

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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
Expand Down
6 changes: 3 additions & 3 deletions src/main/sensors/gyro.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
}
}
Expand Down Expand Up @@ -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]));
}
}

Expand Down

0 comments on commit 1dddd65

Please sign in to comment.