More efficiency in PID code // add yaw velocity to setpoint

src/main/config/config.c

@@ -245,8 +245,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     // Betaflight PID controller parameters
     pidProfile->ptermSetpointWeight = 75;
     pidProfile->dtermSetpointWeight = 200;
-    pidProfile->pidMaxVelocity = 1000;
+    pidProfile->pidMaxVelocityYaw = 200;
-    pidProfile->pidMaxVelocityYaw = 50;
     pidProfile->toleranceBand = 20;
     pidProfile->toleranceBandReduction = 40;
     pidProfile->zeroCrossAllowanceCount = 2;

src/main/flight/pid.c

@@ -133,13 +133,10 @@ 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], yawMaxVelocity, yawPreviousRate;
     float delta;
     int axis;
     float horizonLevelStrength = 1;
-    static int16_t axisPIDState[3];
-    static float velocityWindupFactor[3] = { 1.0f, 1.0f, 1.0f };
-
-    const float velocityFactor = getdT() * 1000.0f;
 
     float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
 
@@ -180,13 +177,31 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
     // ----------PID controller----------
     for (axis = 0; axis < 3; axis++) {
 
+        static uint8_t configP[3], configI[3], configD[3];
+
+        // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now
         // Prepare all parameters for PID controller
-        float Kp = PTERM_SCALE * pidProfile->P8[axis];
+        if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) {
-        float Ki = ITERM_SCALE * pidProfile->I8[axis];
+            Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
-        float Kd = DTERM_SCALE * pidProfile->D8[axis];
+            Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
-        float b = pidProfile->ptermSetpointWeight / 100.0f;
+            Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
-        float c = pidProfile->dtermSetpointWeight / 100.0f;
+            b[axis] = pidProfile->ptermSetpointWeight / 100.0f;
-        float velocityMax = (axis == YAW) ? (float)pidProfile->pidMaxVelocityYaw * velocityFactor : (float)pidProfile->pidMaxVelocity * velocityFactor;
+            c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
+            yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT();
+
+            configP[axis] = pidProfile->P8[axis];
+            configI[axis] = pidProfile->I8[axis];
+            configD[axis] = pidProfile->D8[axis];
+        }
+
+        // Limit abrupt yaw inputs
+        if (axis == YAW && pidProfile->pidMaxVelocityYaw) {
+            float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate;
+            if (ABS(yawCurrentVelocity) > yawMaxVelocity) {
+                setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity;
+            }
+            yawPreviousRate = setpointRate[axis];
+        }
 
         // Yaw control is GYRO based, direct sticks control is applied to rate PID
         if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
@@ -214,9 +229,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
         //  ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error).  ----------
         // 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
+        errorRate = setpointRate[axis] - PVRate;       // r - y
-        rP = b * setpointRate[axis] - PVRate;    // br - y
+        rP = b[axis] * setpointRate[axis] - PVRate;    // br - y
-        rD = c * setpointRate[axis] - PVRate;    // cr - y
 
         // Slowly restore original setpoint with more stick input
         float diffRate = errorRate - rP;
@@ -251,7 +265,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
         }
 
         // -----calculate P component
-        PTerm = Kp * rP * dynReduction;
+        PTerm = Kp[axis] * rP * dynReduction;
 
         // -----calculate I component.
         // Reduce strong Iterm accumulation during higher stick inputs
@@ -260,9 +274,9 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
 
         // Handle All windup Scenarios
         // limit maximum integrator value to prevent WindUp
-        float itermScaler = setpointRateScaler * kiThrottleGain * velocityWindupFactor[axis];
+        float itermScaler = setpointRateScaler * kiThrottleGain;
 
-        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki * errorRate * getdT() * itermScaler, -250.0f, 250.0f);
+        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f);
 
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
         ITerm = errorGyroIf[axis];
@@ -275,13 +289,14 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
 
             DTerm = 0.0f; // needed for blackbox
         } else {
+            rD = c[axis] * setpointRate[axis] - PVRate;    // cr - y
             delta = rD - lastRateError[axis];
             lastRateError[axis] = rD;
 
             // Divide delta by targetLooptime to get differential (ie dr/dt)
             delta *= (1.0f / getdT());
 
-            if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd * delta * dynReduction;
+            if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction;
 
             // Filter delta
             if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
@@ -294,30 +309,15 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
                 }
             }
 
-            DTerm = constrainf(Kd * delta * dynReduction, -300.0f, 300.0f);
+            DTerm = Kd[axis] * delta * dynReduction;
 
             // -----calculate total PID output
-            axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
+            axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900);
         }
 
         // Disable PID control at zero throttle
         if (!pidStabilisationEnabled) axisPID[axis] = 0;
 
-        // Velocity limit only active below 1000
-        if (velocityMax < 1000) {
-            int16_t currentVelocity = axisPID[axis] - axisPIDState[axis];
-            if (debugMode == DEBUG_VELOCITY) debug[axis] = currentVelocity;
-            if (ABS(currentVelocity) > velocityMax) {
-                axisPID[axis] = (currentVelocity > 0) ? axisPIDState[axis] + velocityMax : axisPIDState[axis] - velocityMax;
-                velocityWindupFactor[axis] = ABS(currentVelocity) / velocityMax;
-            }
-            else {
-                velocityWindupFactor[axis] = 1.0f;
-            }
-
-            axisPIDState[axis] = axisPID[axis];
-        }
-
 #ifdef GTUNE
         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
             calculate_Gtune(axis);

src/main/flight/pid.h

@@ -100,8 +100,7 @@ typedef struct pidProfile_s {
     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 dtermSetpointWeight;            // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
-    uint16_t pidMaxVelocity;                // velocity limiter for pid controller (per ms)
+    uint16_t pidMaxVelocityYaw;             // velocity limiter for pid controller deg/sec/ms
-    uint16_t pidMaxVelocityYaw;             // velocity limiter for pid controller (per ms) yaw
 
 #ifdef GTUNE
     uint8_t  gtune_lolimP[3];               // [0..200] Lower limit of P during G tune

src/main/io/serial_cli.c

@@ -832,9 +832,8 @@ const clivalue_t valueTable[] = {
     { "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 = {30, 100 } },
-    { "dterm_setpoint_weight",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } },
+    { "dterm_setpoint_weight",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } },
-    { "pid_max_velocity",           VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocity, .config.minmax = {0, 2000 } },
+    { "max_yaw_acceleration",       VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 1000 } },
-    { "pid_max_velocity_yaw",       VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 2000 } },
 
     { "iterm_ignore_threshold",     VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
     { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },

src/main/io/serial_msp.c

@@ -1268,8 +1268,8 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize8(currentProfile->pidProfile.toleranceBand);
         serialize8(currentProfile->pidProfile.toleranceBandReduction);
         serialize8(currentProfile->pidProfile.itermThrottleGain);
-        serialize16(currentProfile->pidProfile.pidMaxVelocity);
         serialize16(currentProfile->pidProfile.pidMaxVelocityYaw);
+        serialize16(1000); // Reserved for future
         break;
     case MSP_SENSOR_CONFIG:
         headSerialReply(3);
@@ -1858,8 +1858,8 @@ static bool processInCommand(void)
         currentProfile->pidProfile.toleranceBand = read8();
         currentProfile->pidProfile.toleranceBandReduction = read8();
         currentProfile->pidProfile.itermThrottleGain = read8();
-        currentProfile->pidProfile.pidMaxVelocity = read16();
         currentProfile->pidProfile.pidMaxVelocityYaw = read16();
+        read16();  // reserved for future purposes
         break;
     case MSP_SET_SENSOR_CONFIG:
         masterConfig.acc_hardware = read8();

src/main/mw.c

@@ -240,7 +240,6 @@ void processRcCommand(void)
                 debug[3] = rxRefreshRate;
             }
 
-            isRXDataNew = false;
             for (int channel=0; channel < 4; channel++) {
                 deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
                 lastCommand[channel] = rcCommand[channel];
@@ -264,9 +263,11 @@ void processRcCommand(void)
     }
 
     if (readyToCalculateRate || isRXDataNew) {
-        isRXDataNew = false;
+        // Don't smooth yaw axis
+        int axisToCalculate = (isRXDataNew) ? 3 : 2;
+        for (int axis = 0; axis < axisToCalculate; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]);
 
-        for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]);
+        isRXDataNew = false;
 
         // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
         if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))