Reimplementation of P setpoint weight. Applied to S rates now for nicer feel

src/main/blackbox/blackbox.c

@@ -1283,7 +1283,7 @@ static bool blackboxWriteSysinfo()
         BLACKBOX_PRINT_HEADER_LINE("toleranceBandReduction:%d",           masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBandReduction);
         BLACKBOX_PRINT_HEADER_LINE("zeroCrossAllowanceCount:%d",          masterConfig.profile[masterConfig.current_profile_index].pidProfile.zeroCrossAllowanceCount);
         BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d",                masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermThrottleGain);
-        BLACKBOX_PRINT_HEADER_LINE("ptermSetpointWeight:%d",              masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSetpointWeight);
+        BLACKBOX_PRINT_HEADER_LINE("ptermSRateWeight:%d",                 masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSRateWeight);
         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

@@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 145;
+static const uint8_t EEPROM_CONF_VERSION = 146;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -235,7 +235,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->D8[PIDVEL] = 75;
 
     pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
-    pidProfile->yaw_lpf_hz = 80;
+    pidProfile->yaw_lpf_hz = 0;
     pidProfile->rollPitchItermIgnoreRate = 130;
     pidProfile->yawItermIgnoreRate = 32;
     pidProfile->dterm_filter_type = FILTER_BIQUAD;
@@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
 
     // Betaflight PID controller parameters
-    pidProfile->ptermSetpointWeight = 85;
+    pidProfile->ptermSRateWeight = 50;
     pidProfile->dtermSetpointWeight = 150;
     pidProfile->yawRateAccelLimit = 220;
     pidProfile->rateAccelLimit = 0;
@@ -491,8 +491,8 @@ void createDefaultConfig(master_t *config)
 #endif
     config->gyro_soft_type = FILTER_PT1;
     config->gyro_soft_lpf_hz = 90;
-    config->gyro_soft_notch_hz = 0;
+    config->gyro_soft_notch_hz = 210;
-    config->gyro_soft_notch_cutoff = 150;
+    config->gyro_soft_notch_cutoff = 110;
 
     config->debug_mode = DEBUG_NONE;
 

src/main/flight/pid.c

@@ -49,7 +49,7 @@
 
 extern uint8_t motorCount;
 uint32_t targetPidLooptime;
-extern float setpointRate[3];
+extern float setpointRate[3], ptermSetpointRate[3];
 extern float rcInput[3];
 
 static bool pidStabilisationEnabled;
@@ -135,7 +135,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
     float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
     float ITerm,PTerm,DTerm;
     static float lastRateError[2];
-    static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
+    static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3];
     float delta;
     int axis;
     float horizonLevelStrength = 1;
@@ -187,7 +187,6 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
             Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
             Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
             Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
-            b[axis] = pidProfile->ptermSetpointWeight / 100.0f;
             c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
             yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT();
             rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
@@ -234,11 +233,7 @@ 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 = b[axis] * setpointRate[axis] - PVRate;    // br - y
+        rP = ptermSetpointRate[axis] - PVRate;         // br - y
-
-        // Slowly restore original setpoint with more stick input
-        float diffRate = errorRate - rP;
-        rP += diffRate * rcInput[axis];
 
         // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount
         float dynReduction = tpaFactor;

src/main/flight/pid.h

@@ -98,7 +98,7 @@ typedef struct pidProfile_s {
     uint8_t toleranceBandReduction;         // Lowest possible P and D reduction in percentage
     uint8_t zeroCrossAllowanceCount;        // Amount of bouncebacks within tolerance band allowed before reduction kicks in
     uint8_t itermThrottleGain;              // Throttle coupling to iterm. Quick throttle changes will bump iterm
-    uint8_t ptermSetpointWeight;            // Setpoint weight for Pterm (lower means more PV tracking)
+    uint8_t ptermSRateWeight;               // Setpoint super expo ratio for Pterm (lower means that pretty much only P has super expo rates)
     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/serial_cli.c

@@ -854,7 +854,7 @@ const clivalue_t valueTable[] = {
     { "tolerance_band_reduction",   VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } },
     { "zero_cross_allowance",       VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } },
     { "iterm_throttle_gain",        VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
-    { "pterm_setpoint_weight",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 100 } },
+    { "pterm_srate_ratio",          VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSRateWeight, .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

@@ -1268,7 +1268,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize16(currentProfile->pidProfile.yaw_p_limit);
         serialize8(currentProfile->pidProfile.deltaMethod);
         serialize8(currentProfile->pidProfile.vbatPidCompensation);
-        serialize8(currentProfile->pidProfile.ptermSetpointWeight);
+        serialize8(currentProfile->pidProfile.ptermSRateWeight);
         serialize8(currentProfile->pidProfile.dtermSetpointWeight);
         serialize8(currentProfile->pidProfile.toleranceBand);
         serialize8(currentProfile->pidProfile.toleranceBandReduction);
@@ -1866,7 +1866,7 @@ static bool processInCommand(void)
         currentProfile->pidProfile.yaw_p_limit = read16();
         currentProfile->pidProfile.deltaMethod = read8();
         currentProfile->pidProfile.vbatPidCompensation = read8();
-        currentProfile->pidProfile.ptermSetpointWeight = read8();
+        currentProfile->pidProfile.ptermSRateWeight = read8();
         currentProfile->pidProfile.dtermSetpointWeight = read8();
         currentProfile->pidProfile.toleranceBand = read8();
         currentProfile->pidProfile.toleranceBandReduction = read8();

src/main/mw.c

@@ -123,7 +123,7 @@ extern uint8_t PIDweight[3];
 uint16_t filteredCycleTime;
 static bool isRXDataNew;
 static bool armingCalibrationWasInitialised;
-float setpointRate[3];
+float setpointRate[3], ptermSetpointRate[3];
 float rcInput[3];
 
 extern pidControllerFuncPtr pid_controller;
@@ -176,7 +176,7 @@ bool isCalibrating()
 #define RC_RATE_INCREMENTAL 14.54f
 #define RC_EXPO_POWER 3
 
-float calculateSetpointRate(int axis, int16_t rc) {
+void calculateSetpointRate(int axis, int16_t rc) {
     float angleRate, rcRate, rcSuperfactor, rcCommandf;
     uint8_t rcExpo;
 
@@ -201,7 +201,19 @@ float 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));
-        angleRate *= rcSuperfactor;
+        if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) {
+            ptermSetpointRate[axis] = angleRate * rcSuperfactor;
+            if (currentProfile->pidProfile.ptermSRateWeight < 100) {
+                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) {
@@ -209,9 +221,9 @@ float calculateSetpointRate(int axis, int16_t rc) {
     }
 
     if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY)
-        return  constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection
+        setpointRate[axis] = constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection
     else
-        return  constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec)
+        setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec)
 }
 
 void scaleRcCommandToFpvCamAngle(void) {
@@ -290,7 +302,7 @@ void processRcCommand(void)
         if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
             scaleRcCommandToFpvCamAngle();
 
-        for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]);
+        for (int axis = 0; axis < 3; axis++) calculateSetpointRate(axis, rcCommand[axis]);
 
         isRXDataNew = false;
     }