diff --git a/src/main/config/config.c b/src/main/config/config.c index 7cbc761485..ddf15f8c7e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 131; +static const uint8_t EEPROM_CONF_VERSION = 132; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -178,7 +178,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 70.0f; - pidProfile->dterm_average_count = 4; + pidProfile->dterm_average_count = 0; + pidProfile->dynamic_dterm_threshold = 20; 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 b54b058b55..a343d0c733 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -133,6 +133,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa static float lastErrorForDelta[3]; static float deltaState[3][DELTA_MAX_SAMPLES]; float delta; + float dynamicDTermGain; int axis; float horizonLevelStrength = 1; @@ -141,7 +142,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // Scaling factors for Pids to match rewrite and use same defaults static const float luxPTermScale = 1.0f / 128; static const float luxITermScale = 1000000.0f / 0x1000000; - static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 256; + static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions @@ -245,6 +246,8 @@ 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 / getdT()); + // Several different Dterm filtering methods to prevent noise. All of them can be combined + // Filter delta if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); @@ -252,6 +255,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa 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 @@ -283,6 +292,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co static int32_t lastErrorForDelta[3] = { 0, 0, 0 }; static int32_t deltaState[3][DELTA_MAX_SAMPLES]; int32_t AngleRateTmp, RateError, gyroRate; + int32_t dynamicDTermGain; int8_t horizonLevelStrength = 100; @@ -389,15 +399,23 @@ 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 / ((uint16_t)targetPidLooptime >> 4))) >> 6; + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + + // Several different Dterm filtering methods to prevent noise. All of them can be combined // 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]) * 2; + 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 dfcce336fc..8fddd77bb3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -71,6 +71,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; #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 b2f19e1c85..d9611bb1f8 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -722,6 +722,7 @@ const clivalue_t valueTable[] = { { "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 } }, { "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 } }, diff --git a/src/main/version.h b/src/main/version.h index 799c7d43b4..587b442c73 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x)