Move PID relaxation to only D

src/main/blackbox/blackbox.c

@@ -1280,7 +1280,7 @@ static bool blackboxWriteSysinfo()
 
         // Betaflight PID controller parameters
         BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d",                masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermThrottleGain);
-        BLACKBOX_PRINT_HEADER_LINE("ptermSRateWeight:%d",                 masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSRateWeight);
+        BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d",               masterConfig.profile[masterConfig.current_profile_index].pidProfile.setpointRelaxRatio);
         BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d",              masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight);
         BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d",                masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawRateAccelLimit);
         BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d",                   masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit);

src/main/config/config.c

@@ -247,8 +247,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
 
     // Betaflight PID controller parameters
-    pidProfile->ptermSRateWeight = 85;
+    pidProfile->setpointRelaxRatio = 70;
-    pidProfile->dtermSetpointWeight = 150;
+    pidProfile->dtermSetpointWeight = 200;
     pidProfile->yawRateAccelLimit = 220;
     pidProfile->rateAccelLimit = 0;
     pidProfile->itermThrottleGain = 0;

src/main/flight/pid.c

@@ -49,7 +49,7 @@
 
 extern uint8_t motorCount;
 uint32_t targetPidLooptime;
-extern float setpointRate[3], ptermSetpointRate[3];
+extern float setpointRate[3];
 extern float rcInput[3];
 
 static bool pidStabilisationEnabled;
@@ -132,10 +132,10 @@ void initFilters(const pidProfile_t *pidProfile) {
 static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
          const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
 {
-    float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
+    float errorRate = 0, rD = 0, PVRate = 0, dynC;
     float ITerm,PTerm,DTerm;
     static float lastRateError[2];
-    static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
+    static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3];
     float delta;
     int axis;
     float horizonLevelStrength = 1;
@@ -188,6 +188,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
             Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
             Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
             c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
+            relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f);
             yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT();
             rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
 
@@ -218,11 +219,11 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
 #endif
             if (FLIGHT_MODE(ANGLE_MODE)) {
                 // ANGLE mode - control is angle based, so control loop is needed
-                ptermSetpointRate[axis] = setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
+                setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
             } else {
                 // HORIZON mode - direct sticks control is applied to rate PID
                 // mix up angle error to desired AngleRate to add a little auto-level feel
-                ptermSetpointRate[axis] = setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
+                setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
             }
         }
 
@@ -233,10 +234,9 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
         // ----- calculate error / angle rates  ----------
         errorRate = setpointRate[axis] - PVRate;       // r - y
-        rP = ptermSetpointRate[axis] - PVRate;         // br - y
 
-        // -----calculate P component
+        // -----calculate P component and add Dynamic Part based on stick input
-        PTerm = Kp[axis] * rP * tpaFactor;
+        PTerm = Kp[axis] * errorRate * tpaFactor;
 
         // -----calculate I component.
         // Reduce strong Iterm accumulation during higher stick inputs
@@ -254,7 +254,11 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
 
         //-----calculate D-term (Yaw D not yet supported)
         if (axis != YAW) {
-            rD = c[axis] * setpointRate[axis] - PVRate;    // cr - y
+            if (pidProfile->setpointRelaxRatio < 100)
+                dynC = c[axis] * powerf(rcInput[axis], 2) * relaxFactor[axis] + c[axis] * (1-relaxFactor[axis]);
+            else
+                dynC = c[axis];
+            rD = dynC * setpointRate[axis] - PVRate;    // cr - y
             delta = rD - lastRateError[axis];
             lastRateError[axis] = rD;
 

src/main/flight/pid.h

@@ -90,7 +90,7 @@ typedef struct pidProfile_s {
 
     // Betaflight PID controller parameters
     uint8_t itermThrottleGain;              // Throttle coupling to iterm. Quick throttle changes will bump iterm
-    uint8_t ptermSRateWeight;               // Setpoint super expo ratio for Pterm (lower means that pretty much only P has super expo rates)
+    uint8_t setpointRelaxRatio;             // Setpoint weight relaxation effect
     uint8_t dtermSetpointWeight;            // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
     uint16_t yawRateAccelLimit;             // yaw accel limiter for deg/sec/ms
     uint16_t rateAccelLimit;                // accel limiter roll/pitch deg/sec/ms

src/main/io/osd.c

@@ -477,7 +477,7 @@ static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10};
 static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10};
 static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10};
 static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10};
-static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.ptermSRateWeight, 0, 100, 1, 10};
+static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10};
 static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10};
 
 OSD_Entry menuRateExpo[] =

src/main/io/serial_cli.c

@@ -851,7 +851,7 @@ const clivalue_t valueTable[] = {
     { "vbat_pid_compensation",      VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
     { "pid_at_min_throttle",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } },
     { "iterm_throttle_gain",        VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
-    { "pterm_srate_ratio",          VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSRateWeight, .config.minmax = {0, 100 } },
+    { "setpoint_relax_ratio",       VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } },
     { "dterm_setpoint_weight",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } },
     { "yaw_rate_accel_limit",       VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } },
     { "rate_accel_limit",           VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } },

src/main/io/serial_msp.c

@@ -1274,7 +1274,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize16(currentProfile->pidProfile.yaw_p_limit);
         serialize8(currentProfile->pidProfile.deltaMethod);
         serialize8(currentProfile->pidProfile.vbatPidCompensation);
-        serialize8(currentProfile->pidProfile.ptermSRateWeight);
+        serialize8(currentProfile->pidProfile.setpointRelaxRatio);
         serialize8(currentProfile->pidProfile.dtermSetpointWeight);
         serialize8(0); // reserved
         serialize8(0); // reserved
@@ -1877,7 +1877,7 @@ static bool processInCommand(void)
         currentProfile->pidProfile.yaw_p_limit = read16();
         currentProfile->pidProfile.deltaMethod = read8();
         currentProfile->pidProfile.vbatPidCompensation = read8();
-        currentProfile->pidProfile.ptermSRateWeight = read8();
+        currentProfile->pidProfile.setpointRelaxRatio = read8();
         currentProfile->pidProfile.dtermSetpointWeight = read8();
         read8(); // reserved
         read8(); // reserved

src/main/mw.c

@@ -122,7 +122,7 @@ extern uint8_t PIDweight[3];
 
 static bool isRXDataNew;
 static bool armingCalibrationWasInitialised;
-float setpointRate[3], ptermSetpointRate[3];
+float setpointRate[3];
 float rcInput[3];
 
 extern pidControllerFuncPtr pid_controller;
@@ -200,19 +200,7 @@ void calculateSetpointRate(int axis, int16_t rc) {
 
     if (currentControlRateProfile->rates[axis]) {
         rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
-        if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) {
+        angleRate *= rcSuperfactor;
-            ptermSetpointRate[axis] = constrainf(angleRate * rcSuperfactor, -1998.0f, 1998.0f);
-            if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW && !flightModeFlags) {
-                const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f;
-                angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate);
-            } else {
-                angleRate = ptermSetpointRate[axis];
-            }
-        } else {
-            angleRate *= rcSuperfactor;
-        }
-    } else {
-        if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) ptermSetpointRate[axis] = angleRate;
     }
 
     if (debugMode == DEBUG_ANGLERATE) {