Merge pull request #1707 from betaflight/pid_config_init

Add PID config initialisation

src/main/cms/cms_menu_imu.c

@@ -130,6 +130,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
         masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1];
         masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2];
     }
+    pidInitConfig(&currentProfile->pidProfile);
 
     return 0;
 }
@@ -248,6 +249,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
 
     masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight;
     masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio;
+    pidInitConfig(&currentProfile->pidProfile);
 
     masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
     masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;

src/main/fc/fc_msp.c

@@ -1264,6 +1264,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             currentProfile->pidProfile.I8[i] = sbufReadU8(src);
             currentProfile->pidProfile.D8[i] = sbufReadU8(src);
         }
+        pidInitConfig(&currentProfile->pidProfile);
         break;
 
     case MSP_SET_MODE_RANGE:
@@ -1478,6 +1479,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         currentProfile->pidProfile.itermThrottleGain = sbufReadU8(src);
         currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src);
         currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src);
+        pidInitConfig(&currentProfile->pidProfile);
         break;
 
     case MSP_SET_SENSOR_CONFIG:

src/main/fc/rc_controls.c

@@ -471,6 +471,16 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
         .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
         .mode = ADJUSTMENT_MODE_STEP,
         .data = { .stepConfig = { .step = 1 }}
+    },
+    {
+        .adjustmentFunction = ADJUSTMENT_D_SETPOINT,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    },
+    {
+        .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
     }
 };
 
@@ -579,7 +589,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
         case ADJUSTMENT_ROLL_D:
             newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
             pidProfile->D8[PIDROLL] = newValue;
-                blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
+            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
             break;
         case ADJUSTMENT_YAW_P:
             newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
@@ -601,6 +611,14 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
             controlRateConfig->rcYawRate8 = newValue;
             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
             break;
+        case ADJUSTMENT_D_SETPOINT:
+            newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in serial_cli.c
+            pidProfile->dtermSetpointWeight = newValue;
+            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
+        case ADJUSTMENT_D_SETPOINT_TRANSITION:
+            newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            pidProfile->setpointRelaxRatio = newValue;
+            blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
         default:
             break;
     };
@@ -676,7 +694,8 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx
                 continue;
             }
 
-            applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
+            applyStepAdjustment(controlRateConfig,adjustmentFunction,delta);
+            pidInitConfig(pidProfile);
         } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
             uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
             uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;

src/main/fc/rc_controls.h

@@ -205,6 +205,8 @@ typedef enum {
     ADJUSTMENT_ROLL_I,
     ADJUSTMENT_ROLL_D,
     ADJUSTMENT_RC_RATE_YAW,
+    ADJUSTMENT_D_SETPOINT,
+    ADJUSTMENT_D_SETPOINT_TRANSITION,
     ADJUSTMENT_FUNCTION_COUNT,
 
 } adjustmentFunction_e;

src/main/flight/pid.c

@@ -54,17 +54,20 @@ uint8_t PIDweight[3];
 int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 #endif
 
-static float errorGyroIf[3];
+static float previousGyroIf[3];
+
+static float dT;
 
 void pidSetTargetLooptime(uint32_t pidLooptime)
 {
     targetPidLooptime = pidLooptime;
+    dT = (float)targetPidLooptime * 0.000001f;
 }
 
 void pidResetErrorGyroState(void)
 {
     for (int axis = 0; axis < 3; axis++) {
-        errorGyroIf[axis] = 0.0f;
+        previousGyroIf[axis] = 0.0f;
     }
 }
 
@@ -91,7 +94,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     static pt1Filter_t pt1FilterYaw;
 
     BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
-    const float dT = (float)targetPidLooptime * 0.000001f;
 
     if (pidProfile->dterm_notch_hz == 0) {
         dtermNotchFilterApplyFn = nullFilterApply;
@@ -144,20 +146,28 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     }
 }
 
+static float Kp[3], Ki[3], Kd[3], c[3];
+static float rollPitchMaxVelocity, yawMaxVelocity, relaxFactor[3];
+
+void pidInitConfig(const pidProfile_t *pidProfile) {
+    for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+        Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
+        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 * dT;
+    rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
+}
+
 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 // Based on 2DOF reference design (matlab)
 void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
          const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
 {
-    static float lastRateError[2];
+    static float previousRateError[2];
-    static float Kp[3], Ki[3], Kd[3], c[3];
+    static float previousSetpoint[3];
-    static float rollPitchMaxVelocity, yawMaxVelocity;
-    static float previousSetpoint[3], relaxFactor[3];
-    static float dT;
-
-    if (!dT) {
-        dT = (float)targetPidLooptime * 0.000001f;
-    }
 
     float horizonLevelStrength = 1;
     if (FLIGHT_MODE(HORIZON_MODE)) {
@@ -195,25 +205,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
     // ----------PID controller----------
     const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
     for (int axis = FD_ROLL; axis <= FD_YAW; 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
-        if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) {
-            Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
-            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 * dT;
-            rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
-
-            configP[axis] = pidProfile->P8[axis];
-            configI[axis] = pidProfile->I8[axis];
-            configD[axis] = pidProfile->D8[axis];
-        }
-
         // Limit abrupt yaw inputs / stops
         const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
         if (maxVelocity) {
@@ -221,7 +212,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
             if (ABS(currentVelocity) > maxVelocity) {
                 setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
             }
-            previousSetpoint[axis] = setpointRate[axis];
         }
 
         // Yaw control is GYRO based, direct sticks control is applied to rate PID
@@ -261,16 +251,15 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
         const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
         const float itermScaler = setpointRateScaler * kiThrottleGain;
 
-        float ITerm = errorGyroIf[axis];
+        float ITerm = previousGyroIf[axis];
         ITerm += Ki[axis] * errorRate * dT * itermScaler;;
         // limit maximum integrator value to prevent WindUp
         ITerm = constrainf(ITerm, -250.0f, 250.0f);
-        errorGyroIf[axis] = ITerm;
+        previousGyroIf[axis] = ITerm;
 
         // -----calculate D component (Yaw D not yet supported)
         float DTerm = 0.0;
         if (axis != FD_YAW) {
-            static float previousSetpoint[3];
             float dynC = c[axis];
             if (pidProfile->setpointRelaxRatio < 100) {
                 dynC = c[axis];
@@ -282,11 +271,10 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
                         dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
                 }
             }
-            previousSetpoint[axis] = setpointRate[axis];
             const float rD = dynC * setpointRate[axis] - PVRate;    // cr - y
             // Divide rate change by dT to get differential (ie dr/dt)
-            const float delta = (rD - lastRateError[axis]) / dT;
+            const float delta = (rD - previousRateError[axis]) / dT;
-            lastRateError[axis] = rD;
+            previousRateError[axis] = rD;
 
             DTerm = Kd[axis] * delta * tpaFactor;
             DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
@@ -298,6 +286,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
         } else {
             PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
         }
+        previousSetpoint[axis] = setpointRate[axis];
 
         // -----calculate total PID output
         axisPIDf[axis] = PTerm + ITerm + DTerm;

src/main/flight/pid.h

@@ -106,4 +106,5 @@ void pidResetErrorGyroState(void);
 void pidStabilisationState(pidStabilisationState_e pidControllerState);
 void pidSetTargetLooptime(uint32_t pidLooptime);
 void pidInitFilters(const pidProfile_t *pidProfile);
+void pidInitConfig(const pidProfile_t *pidProfile);
 

src/main/main.c

@@ -451,6 +451,7 @@ void init(void)
     // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime()
     pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
     pidInitFilters(&currentProfile->pidProfile);
+    pidInitConfig(&currentProfile->pidProfile);
 
     imuInit();
 

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -708,6 +708,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
                 bstWrite8(currentProfile->pidProfile.I8[i]);
                 bstWrite8(currentProfile->pidProfile.D8[i]);
             }
+            pidInitConfig(&currentProfile->pidProfile);
             break;
         case BST_PIDNAMES:
             bstWriteNames(pidnames);