Skip to content

Commit

Permalink
Merge branch 'filter_biquad' of https://github.com/borisbstyle/betafl…
Browse files Browse the repository at this point in the history 
…ight into borisbstyle-filter_biquad

Conflicts:
	src/main/flight/pid.c
	src/main/flight/pid.h
	src/test/unit/pid_unittest.cc
  • Loading branch information
@hydra
hydra committed Feb 4, 2016
2 parents 25c7db4 + 5640d54 commit b06b7f1
Show file tree
Hide file tree
Showing 10 changed files with 138 additions and 130 deletions.
112 changes: 55 additions & 57 deletions src/main/common/filter.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,12 @@
#include "common/filter.h"
#include "common/maths.h"

#include "drivers/gyro_sync.h"

#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f

#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */

// 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) {
Expand All @@ -38,65 +44,57 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
return filter->state;
}

/**
* Typical quadcopter motor noise frequency (at 50% throttle):
* 450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz
* 250-sized, 2300kv, 5x4.5 props, 4S : 14139rpm = 235Hz
*/
static int8_t gyroFIRCoeff_500[3][FILTER_TAPS] = { { 0, 18, 14, 16, 20, 22, 24, 25, 25, 24, 20, 18, 12, 18 }, // TODO - NEEDS TABLE
{ 23, 12, 16, 18, 20, 20, 23, 20, 22, 18, 16, 14, 12, 22 }, //
{ 36, 12, 14, 14, 16, 16, 18, 18, 18, 16, 16, 14, 12, 36 } };//
static int8_t gyroFIRCoeff_1000[3][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
{ 0, 0, 0, 0, 0, 18, 30, 42, 46, 40, 34, 22, 8, 8}, // looptime=1000; group delay 3ms; -0.5db = 18Hz ; -1db = 33Hz; -5db = 81Hz; -10db = 113Hz
{ 0, 0, 0, 0, 0, 18, 12, 28, 40, 44, 40, 32, 22, 20} };// looptime=1000; group delay 4ms; -0.5db = 23Hz ; -1db = 35Hz; -5db = 75Hz; -10db = 103Hz
static int8_t gyroFIRCoeff_2000[3][FILTER_TAPS] = { { 0, 0, 0, 0, 0, 0, 0, 0, 6, 24, 58, 82, 64, 20 }, // looptime=2000, group delay 4ms; -0.5db = 21Hz ; -1db = 31Hz; -5db = 71Hz; -10db = 99Hz
{ 0, 0, 0, 0, 0, 0, 0, 14, 22, 46, 60, 56, 40, 24}, // looptime=2000, group delay 5ms; -0.5db = 20Hz ; -1db = 26Hz; -5db = 52Hz; -10db = 71Hz
{ 0, 0, 0, 0, 0, 14, 12, 26, 38, 44, 42, 34, 24, 20} };// looptime=2000, group delay 7ms; -0.5db = 11Hz ; -1db = 18Hz; -5db = 38Hz; -10db = 52Hz
static int8_t gyroFIRCoeff_3000[3][FILTER_TAPS] = { { 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 36, 88, 88, 44 }, // looptime=3000, group delay 4.5ms; -0.5db = 18Hz ; -1db = 26Hz; -5db = 57Hz; -10db = 78Hz
{ 0, 0, 0, 0, 0, 0, 0, 0, 14, 32, 64, 72, 54, 28}, // looptime=3000, group delay 6.5ms; -0.5db = 16Hz ; -1db = 21Hz; -5db = 42Hz; -10db = 57Hz
{ 0, 0, 0, 0, 0, 0, 6, 10, 28, 44, 54, 54, 38, 22} }; // looptime=3000, group delay 9ms; -0.5db = 10Hz ; -1db = 13Hz; -5db = 32Hz; -10db = 45Hz

int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime)
/* sets up a biquad Filter */
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
{
if (filter_level == 0) {
return NULL;
}

int firIndex = constrain(filter_level, 1, 3) - 1;

// For looptimes faster than 700 use filter for 2kHz looptime
if (targetLooptime < 700) {
return gyroFIRCoeff_500[firIndex];
}

// For looptimes faster than 1499 use filter for 1kHz looptime
if (targetLooptime < 1500) {
return gyroFIRCoeff_1000[firIndex];
}
// 1500 ... 2499
else if (targetLooptime < 2500) {
return gyroFIRCoeff_2000[firIndex];
}
// > 2500
else {
return gyroFIRCoeff_3000[firIndex];
}
float sampleRate;

sampleRate = 1 / ((float)refreshRate * 0.000001f);

float omega, sn, cs, alpha;
float a0, a1, a2, b0, b1, b2;

/* setup variables */
omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
sn = sinf(omega);
cs = cosf(omega);
alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);

b0 = (1 - cs) /2;
b1 = 1 - cs;
b2 = (1 - cs) /2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;

/* precompute the coefficients */
newState->b0 = b0 /a0;
newState->b1 = b1 /a0;
newState->b2 = b2 /a0;
newState->a1 = a1 /a0;
newState->a2 = a2 /a0;

/* zero initial samples */
newState->x1 = newState->x2 = 0;
newState->y1 = newState->y2 = 0;
}

// Thanks to Qcopter & BorisB & DigitalEntity
void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS])
/* Computes a biquad_t filter on a sample */
float applyBiQuadFilter(float sample, biquad_t *state)
{
int32_t FIRsum;
int axis, i;

for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
FIRsum = 0;
for (i = 0; i <= FILTER_TAPS-2; i++) {
state[axis][i] = state[axis][i + 1];
FIRsum += state[axis][i] * (int16_t)coeff[i];
}
state[axis][FILTER_TAPS-1] = data[axis];
FIRsum += state[axis][FILTER_TAPS-1] * coeff[FILTER_TAPS-1];
data[axis] = FIRsum / 256;
}
float result;

/* compute result */
result = state->b0 * sample + state->b1 * state->x1 + state->b2 * state->x2 -
state->a1 * state->y1 - state->a2 * 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;
}
12 changes: 8 additions & 4 deletions src/main/common/filter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,18 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/

#define FILTER_TAPS 14

typedef struct filterStatePt1_s {
float state;
float RC;
float constdT;
} filterStatePt1_t;

/* this holds the data required to update samples thru a filter */
typedef struct biquad_s {
float b0, b1, b2, a1, a2;
float x1, x2, y1, y2;
} biquad_t;

float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime);
void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS]);
float applyBiQuadFilter(float sample, biquad_t *state);
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);
11 changes: 5 additions & 6 deletions src/main/config/config.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,8 +178,6 @@ void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 1;

pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->gyro_soft_lpf = 0; // no filtering by default
pidProfile->yaw_pterm_cut_hz = 0;
pidProfile->dterm_cut_hz = 0;
pidProfile->deltaMethod = 1;

Expand Down Expand Up @@ -406,7 +404,8 @@ static void resetConf(void)
masterConfig.current_profile_index = 0; // default profile
masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 3; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
masterConfig.gyro_lpf = 1; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
masterConfig.soft_gyro_lpf_hz = 60; // Software based lpf filter for gyro

resetAccelerometerTrims(&masterConfig.accZero);

Expand Down Expand Up @@ -481,10 +480,10 @@ static void resetConf(void)

resetSerialConfig(&masterConfig.serialConfig);

masterConfig.looptime = 3500;
masterConfig.looptime = 1000;
masterConfig.emf_avoidance = 0;
masterConfig.i2c_overclock = 0;
masterConfig.gyroSync = 0;
masterConfig.gyroSync = 1;
masterConfig.gyroSyncDenominator = 1;

resetPidProfile(&currentProfile->pidProfile);
Expand Down Expand Up @@ -737,7 +736,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);

useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf, masterConfig.looptime));
useGyroConfig(&masterConfig.gyroConfig, masterConfig.soft_gyro_lpf_hz);

#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
Expand Down
1 change: 1 addition & 0 deletions src/main/config/config_master.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ typedef struct master_t {
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
float soft_gyro_lpf_hz; // Software based gyro filter in hz

gyroConfig_t gyroConfig;

Expand Down
76 changes: 36 additions & 40 deletions src/main/flight/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@

#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/gyro_sync.h"

#include "sensors/sensors.h"
#include "sensors/gyro.h"
Expand All @@ -49,7 +50,6 @@
#include "config/runtime_config.h"
#include "config/config_unittest.h"

extern uint16_t cycleTime;
extern uint8_t motorCount;
extern float dT;

Expand Down Expand Up @@ -97,7 +97,16 @@ void pidResetErrorGyro(void)

const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };

static filterStatePt1_t yawPTermState, DTermState[3];
static biquad_t deltaFilterState[3];

void filterIsSetCheck(pidProfile_t *pidProfile) {
static bool deltaStateIsSet;
if (!deltaStateIsSet && pidProfile->dterm_cut_hz) {
int axis;
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_cut_hz, &deltaFilterState[axis], targetLooptime);
deltaStateIsSet = true;
}
}

static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
Expand All @@ -111,6 +120,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
int axis;
float horizonLevelStrength = 1;

filterIsSetCheck(pidProfile);

if (FLIGHT_MODE(HORIZON_MODE)) {

// Figure out the raw stick positions
Expand Down Expand Up @@ -176,11 +187,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;

// Yaw Pterm low pass
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
}

// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -PID_LUX_FLOAT_MAX_I, PID_LUX_FLOAT_MAX_I);

Expand All @@ -201,23 +207,20 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT);

// In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
deltaSum = delta;
} else {
// add moving average here to reduce noise
// When dterm filter disabled apply moving average to reduce noise
if (!pidProfile->dterm_cut_hz) {
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
deltaSum /= 3.0f;
delta = deltaSum / 3.0f;
}

// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
delta = applyBiQuadFilter(delta, &deltaFilterState[axis]);
}

DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -PID_LUX_FLOAT_MAX_D, PID_LUX_FLOAT_MAX_D);
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -PID_LUX_FLOAT_MAX_D, PID_LUX_FLOAT_MAX_D);

// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -PID_LUX_FLOAT_MAX_PID, PID_LUX_FLOAT_MAX_PID);
Expand Down Expand Up @@ -248,6 +251,8 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
static int16_t lastErrorForDelta[2];
static int32_t delta1[2], delta2[2];

filterIsSetCheck(pidProfile);

if (FLIGHT_MODE(HORIZON_MODE)) {
prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
}
Expand Down Expand Up @@ -295,11 +300,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control

PTerm -= ((int32_t)gyroError * dynP8[axis]) >> 6; // 32 bits is needed for calculation

// Yaw Pterm low pass
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
}

//-----calculate D-term based on the configured approach (delta from measurement or deltafromError)
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = error - lastErrorForDelta[axis];
Expand All @@ -309,18 +309,18 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
lastErrorForDelta[axis] = gyroError;
}

// In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
DTerm = delta * 3; // Scaling to match the old pid values
} else {
DTerm = delta1[axis] + delta2[axis] + delta;
// When dterm filter disabled apply moving average to reduce noise
if (!pidProfile->dterm_cut_hz) {
DTerm = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
} else {
DTerm = delta;
}

// Dterm low pass
// Dterm delta low pass
if (pidProfile->dterm_cut_hz) {
DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
DTerm = lrintf(applyBiQuadFilter((float) DTerm, &deltaFilterState[axis])) * 3; // Keep same scaling as unfiltered DTerm
}

DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
Expand Down Expand Up @@ -391,6 +391,8 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
int8_t horizonLevelStrength = 100;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;

filterIsSetCheck(pidProfile);

if (FLIGHT_MODE(HORIZON_MODE)) {

// Figure out the raw stick positions
Expand Down Expand Up @@ -451,17 +453,12 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;

// Yaw Pterm low pass
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
}

// -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis];
errorGyroI[axis] = errorGyroI[axis] + ((RateError * targetLooptime) >> 11) * pidProfile->I8[axis];

// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
Expand All @@ -479,21 +476,20 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co

// 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 / (cycleTime >> 4))) >> 6;
delta = (delta * ((uint16_t) 0xFFFF / (targetLooptime >> 4))) >> 6;

// In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
deltaSum = delta * 3; // Scaling to approximately match the old D values
} else {
// add moving average here to reduce noise
// When dterm filter disabled apply moving average to reduce noise
if (!pidProfile->dterm_cut_hz) {
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
} else {
deltaSum = delta;
}

// Dterm delta low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
deltaSum = lrintf(applyBiQuadFilter((float) deltaSum, &deltaFilterState[axis])) * 3; // Keep same scaling as unfiltered deltaSum
}

DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
Expand Down
4 changes: 1 addition & 3 deletions src/main/flight/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,9 +68,7 @@ typedef struct pidProfile_s {
uint8_t H_sensitivity;

uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t yaw_pterm_cut_hz; // Used for filering Pterm noise on noisy frames
uint8_t gyro_soft_lpf;
float dterm_cut_hz; // dterm filtering
uint8_t deltaMethod; // Alternative delta calculation. Delta from gyro might give smoother results

#ifdef GTUNE
Expand Down
Loading

0 comments on commit b06b7f1

Please sign in to comment.