From cbcf0283028372911b0f9caef4efd25e3ba4c5aa Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 27 Apr 2016 21:08:15 +0200 Subject: [PATCH] Dterm robust differentiator Initial Implementation --- src/main/config/config.c | 2 +- src/main/flight/pid.c | 70 ++++++++++++++++------------------------ src/main/flight/pid.h | 6 +++- src/main/io/serial_cli.c | 3 +- 4 files changed, 35 insertions(+), 46 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ddf15f8c7..ffb211137 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -179,7 +179,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 70.0f; pidProfile->dterm_average_count = 0; - pidProfile->dynamic_dterm_threshold = 20; + pidProfile->dterm_differentiator = 1; pidProfile->rollPitchItermResetRate = 200; pidProfile->yawItermResetRate = 50; pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a343d0c73..37014c05d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -130,10 +130,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa { float RateError, AngleRate, gyroRate; float ITerm,PTerm,DTerm; - static float lastErrorForDelta[3]; - static float deltaState[3][DELTA_MAX_SAMPLES]; + static float lastRate[3][PID_LAST_RATE_COUNT]; float delta; - float dynamicDTermGain; int axis; float horizonLevelStrength = 1; @@ -234,33 +232,28 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); DTerm = 0; } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastErrorForDelta[axis]; - lastErrorForDelta[axis] = RateError; + if (pidProfile->dterm_differentiator) { + // Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) + // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ + // N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4 + delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8; + for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) { + lastRate[axis][i] = lastRate[axis][i-1]; + } } else { - delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastErrorForDelta[axis] = gyroRate; + // When DTerm low pass filter disabled apply moving average to reduce noise + delta = -(gyroRate - lastRate[axis][0]); } - // 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 / getdT()); + lastRate[axis][0] = gyroRate; - // Several different Dterm filtering methods to prevent noise. All of them can be combined + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta *= (1.0f / getdT()); // Filter delta if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); - // Apply moving average - if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2; - DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); - - // Dynamic D Implementation - if (pidProfile->dynamic_dterm_threshold) { - dynamicDTermGain = constrainf(ABS(DTerm) / pidProfile->dynamic_dterm_threshold, 0.0f, 1.0f); - DTerm *= dynamicDTermGain; - } } // -----calculate total PID output @@ -289,10 +282,8 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co int axis; int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastErrorForDelta[3] = { 0, 0, 0 }; - static int32_t deltaState[3][DELTA_MAX_SAMPLES]; + static int32_t lastRate[3][PID_LAST_RATE_COUNT]; int32_t AngleRateTmp, RateError, gyroRate; - int32_t dynamicDTermGain; int8_t horizonLevelStrength = 100; @@ -389,33 +380,28 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); DTerm = 0; } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastErrorForDelta[axis] = RateError; + if (pidProfile->dterm_differentiator) { + // Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) + // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ + // N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4 + delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8; + for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) { + lastRate[axis][i] = lastRate[axis][i-1]; + } } else { - delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastErrorForDelta[axis] = gyroRate; + // When DTerm low pass filter disabled apply moving average to reduce noise + delta = -(gyroRate - lastRate[axis][0]); } - // 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)targetPidLooptime >> 4))) >> 5; + lastRate[axis][0] = gyroRate; - // Several different Dterm filtering methods to prevent noise. All of them can be combined + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; // Filter delta if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); - // Apply moving average - if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]); - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // Dynamic D Implementation - if (pidProfile->dynamic_dterm_threshold) { - dynamicDTermGain = constrain((ABS(DTerm) << 7) / pidProfile->dynamic_dterm_threshold, 0, 1 << 7); - DTerm = (DTerm * dynamicDTermGain) >> 7; - } } // -----calculate total PID output diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 8fddd77bb..2c190feb9 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -22,6 +22,10 @@ #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter +#define PID_LAST_RATE_COUNT 7 +#define PID_DTERM_FIR_MAX_LENGTH 7 +#define PID_MAX_DIFFERENTIATOR (PID_DTERM_FIR_MAX_LENGTH-2) + typedef enum { PIDROLL, PIDPITCH, @@ -71,7 +75,7 @@ typedef struct pidProfile_s { uint8_t deltaMethod; // Alternative delta Calculation uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm - uint8_t dynamic_dterm_threshold; + uint8_t dterm_differentiator; #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9100e03a5..550293fee 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -721,8 +721,7 @@ const clivalue_t valueTable[] = { { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, - { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, - { "dynamic_dterm_threshold", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dynamic_dterm_threshold, .config.minmax = {0, 100 } }, + { "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, { "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },