From 2ee55ece3c0912bac520d519ff91e5234b190bb3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 10 Jan 2016 02:55:21 +0100 Subject: [PATCH] D filter rewrite to FIR --- src/main/common/filter.c | 57 ++++++++++++++++---------- src/main/common/filter.h | 4 +- src/main/flight/pid.c | 86 ++++++++++------------------------------ src/main/io/serial_cli.c | 2 - src/main/sensors/gyro.c | 3 +- 5 files changed, 59 insertions(+), 93 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 144286509b..d2d7f9875c 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -38,37 +38,50 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float return filter->state; } -static int8_t gyroFIRCoeff_500[FILTER_TAPS] = { 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, 12, 23, 40, 51, 52, 40, 38 }; // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz +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 -int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime) + +static int8_t deltaFIRCoeff_500[FILTER_TAPS] = {0, 0, 0, 0, 0, 18, 12, 28, 40, 44, 40, 32, 22, 20}; +static int8_t deltaFIRCoeff_1000[FILTER_TAPS] = {36, 12, 14, 14, 16, 16, 18, 18, 18, 16, 16, 14, 12, 36}; + +int8_t * filterGetFIRCoefficientsTable(uint8_t filter_type, uint32_t targetLooptime) { - if (filter_level == 0) { - return NULL; - } + int8_t *filterCoeff; - // filter for 2kHz looptime - if (targetLooptime == 500) { - return gyroFIRCoeff_500; - } else { // filter for 1kHz looptime - return gyroFIRCoeff_1000; + switch(filter_type){ + case(0): + filterCoeff = NULL; + break; + case(1): + if (targetLooptime == 500) { + filterCoeff = gyroFIRCoeff_500; + } else { // filter for 1kHz looptime + filterCoeff = gyroFIRCoeff_1000; + } + break; + case(2): + if (targetLooptime == 500) { + filterCoeff = deltaFIRCoeff_500; + } else { // filter for 1kHz looptime + filterCoeff = deltaFIRCoeff_1000; + } } + return filterCoeff; } // Thanks to Qcopter & BorisB & DigitalEntity -void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS]) +void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS]) { int32_t FIRsum; - int axis, i; + FIRsum = 0; + int 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; + 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; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 278fbbacc9..a6342eb5e5 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ -#define FILTER_TAPS 13 +#define FILTER_TAPS 14 typedef struct filterStatePt1_s { float state; @@ -25,4 +25,4 @@ typedef struct filterStatePt1_s { 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[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS]); +void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS]); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9287161927..fc202584f0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -64,8 +64,6 @@ uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; static int32_t errorGyroI[3] = { 0, 0, 0 }; static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; -static uint8_t deltaTotalSamples = 0; - static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); @@ -85,18 +83,6 @@ void pidResetErrorGyro(void) errorGyroIf[YAW] = 0.0f; } -void setPidDeltaSamples(uint8_t filter) { - if (!filter) { - if (targetLooptime < 1000) { - deltaTotalSamples = 8; - } else { - deltaTotalSamples = 4; - } - } else { - deltaTotalSamples = 1; - } -} - void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) { float rcCommandReflection = (float)rcCommand[axis] / 500.0f; @@ -137,23 +123,22 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, f const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static filterStatePt1_t DTermState[3]; static airModePlus_t airModePlusAxisState[3]; +static int8_t * deltaFIRTable = 0L; +static int16_t deltaFIRState[3][FILTER_TAPS]; static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { float RateError, AngleRate, gyroRate; float ITerm,PTerm,DTerm; - static float lastGyroRate[3]; static float lastError[3]; - static float previousDelta[3][8]; - float delta, deltaSum; - int axis, deltaCount; + float delta; + int axis; float horizonLevelStrength = 1; static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; - if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz); + if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime); if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions @@ -234,33 +219,20 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa ITerm = errorGyroIf[axis]; //-----calculate D-term - if (pidProfile->delta_from_gyro_error || axis == YAW) { - delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastGyroRate[axis] = gyroRate; - } else { - delta = RateError - lastError[axis]; - lastError[axis] = RateError; - } + delta = RateError - lastError[axis]; + lastError[axis] = RateError; + // 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); - deltaSum = 0; - if (pidProfile->dterm_cut_hz) { - // Dterm low pass - deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); - } + /* Delta filtering is in integers, which should be fine */ + int16_t deltaTemp = (int16_t) delta; + filterApplyFIR(&deltaTemp, deltaFIRState[axis], deltaFIRTable); + delta = (float) deltaTemp; - // Apply moving average - if (deltaTotalSamples > 1) { - for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1]; - previousDelta[axis][0] = delta; - for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount]; - deltaSum = (deltaSum / deltaTotalSamples); - } - - DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); + DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); @@ -288,18 +260,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat { UNUSED(rxConfig); - int axis, deltaCount; - int32_t delta, deltaSum; - static int32_t previousDelta[3][8]; + int axis; + int16_t delta; int32_t PTerm, ITerm, DTerm; static int32_t lastError[3] = { 0, 0, 0 }; - static int32_t lastGyroRate[3] = { 0, 0, 0 }; static int32_t previousErrorGyroI[3] = { 0, 0, 0 }; int32_t AngleRateTmp, RateError, gyroRate; int8_t horizonLevelStrength = 100; - if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz); + if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime); if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions @@ -383,32 +353,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat } //-----calculate D-term - if (pidProfile->delta_from_gyro_error || axis == YAW) { // quick and dirty solution for testing - delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastGyroRate[axis] = gyroRate; - } else { - delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastError[axis] = RateError; - } + delta = (int16_t) RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastError[axis] = RateError; // 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; - // Apply moving average - deltaSum = 0; - if (pidProfile->dterm_cut_hz) { - // Dterm low pass - deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); - } - if (deltaTotalSamples > 1) { - for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1]; - previousDelta[axis][0] = delta; - for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount]; - } - deltaSum = (deltaSum / deltaTotalSamples) * 3; // get old scaling by multiplying with 3 + filterApplyFIR(&delta, deltaFIRState[axis], deltaFIRTable); - DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + DTerm = (delta * 2 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1051a5d177..93ed6bf306 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -656,8 +656,6 @@ const clivalue_t valueTable[] = { { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } }, { "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } }, - { "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } }, - { "delta_from_gyro_error", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.delta_from_gyro_error, .config.minmax = {0, 1} }, { "insane_acrobility_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 30 } }, #ifdef BLACKBOX diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 852f6e9e48..0ad42a6953 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -127,7 +127,8 @@ void gyroUpdate(void) } if (gyroFIRTable) { - filterApplyFIR(gyroADC, gyroFIRState, gyroFIRTable); + int axis; + for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) filterApplyFIR(&gyroADC[axis], gyroFIRState[axis], gyroFIRTable); } alignSensors(gyroADC, gyroADC, gyroAlign);