diff --git a/src/main/common/filter.h b/src/main/common/filter.h index be3bacd852..6f3b588549 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -5,7 +5,7 @@ * Author: borisb */ -#define YAW_PTERM_FILTER 30 + typedef struct filterStatePt1_s { float state; diff --git a/src/main/config/config.c b/src/main/config/config.c index 03e7b96a50..1373ac1238 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -132,7 +132,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 112; +static const uint8_t EEPROM_CONF_VERSION = 113; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -175,6 +175,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 1; pidProfile->dterm_cut_hz = 40; + pidProfile->yaw_pterm_cut_hz = 50; pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully pidProfile->I_f[ROLL] = 0.4f; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 24616c95c5..372caba9d7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -173,8 +173,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate P component PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT); + if (axis == YAW && pidProfile->yaw_pterm_cut_hz) { + PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT); } // -----calculate I component. @@ -289,8 +289,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT); + if (axis == YAW && pidProfile->yaw_pterm_cut_hz) { + PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT); } // -----calculate I component diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c3ca0c7a66..c4e6798de2 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -56,6 +56,7 @@ typedef struct pidProfile_s { uint8_t H_sensitivity; 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; // Yaw P low pass filter for pterm. very usefull on noisy setups #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 64824c6fd6..85ac672aa9 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -520,6 +520,7 @@ const clivalue_t valueTable[] = { { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, { "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 }, + { "yaw_pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, 0, 200 }, #ifdef GTUNE { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], 10, 200 },