Merge pull request #1621 from martinbudden/bf_pid_tidy

Tidy PID controller

src/main/fc/mw.c

@@ -101,8 +101,6 @@ static bool armingCalibrationWasInitialised;
 float setpointRate[3];
 float rcInput[3];
 
-extern pidControllerFuncPtr pid_controller;
-
 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 {
     masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
@@ -695,7 +693,7 @@ void subTaskPidController(void)
         &currentProfile->pidProfile,
         masterConfig.max_angle_inclination,
         &masterConfig.accelerometerTrims,
-        &masterConfig.rxConfig
+        masterConfig.rxConfig.midrc
     );
     if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
 }

src/main/flight/pid.c

@@ -21,7 +21,6 @@
 
 #include <platform.h>
 
-#include "build/build_config.h"
 #include "build/debug.h"
 
 #include "common/axis.h"
@@ -36,10 +35,6 @@
 #include "flight/navigation.h"
 #include "flight/gtune.h"
 
-#include "io/gps.h"
-
-#include "rx/rx.h"
-
 #include "sensors/gyro.h"
 #include "sensors/acceleration.h"
 
@@ -47,8 +42,7 @@ extern float rcInput[3];
 extern float setpointRate[3];
 
 uint32_t targetPidLooptime;
-bool pidStabilisationEnabled;
+static bool pidStabilisationEnabled;
-uint8_t PIDweight[3];
 
 float axisPIDf[3];
 
@@ -59,10 +53,7 @@ uint8_t PIDweight[3];
 int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 #endif
 
-int32_t errorGyroI[3];
+static float errorGyroIf[3];
-float errorGyroIf[3];
-
-pidControllerFuncPtr pid_controller; // which pid controller are we using
 
 void setTargetPidLooptime(uint32_t pidLooptime)
 {
@@ -72,7 +63,6 @@ void setTargetPidLooptime(uint32_t pidLooptime)
 void pidResetErrorGyroState(void)
 {
     for (int axis = 0; axis < 3; axis++) {
-        errorGyroI[axis] = 0;
         errorGyroIf[axis] = 0.0f;
     }
 }
@@ -82,14 +72,6 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
     pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
 }
 
-float getdT(void)
-{
-    static float dT;
-    if (!dT) dT = (float)targetPidLooptime * 0.000001f;
-
-    return dT;
-}
-
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
 pt1Filter_t deltaFilter[3];
@@ -100,23 +82,23 @@ bool dtermNotchInitialised;
 bool dtermBiquadLpfInitialised;
 firFilterDenoise_t dtermDenoisingState[3];
 
-static void pidInitFilters(const pidProfile_t *pidProfile) {
+static void pidInitFilters(const pidProfile_t *pidProfile)
-    int axis;
+{
     static uint8_t lowpassFilterType;
 
     if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
         float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
-        for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
+        for (int axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
         dtermNotchInitialised = true;
     }
 
     if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) {
         if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
-            for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
+            for (int axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
         }
 
         if (pidProfile->dterm_filter_type == FILTER_FIR) {
-            for (axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
+            for (int axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
         }
         lowpassFilterType = pidProfile->dterm_filter_type;
     }
@@ -125,24 +107,25 @@ static void pidInitFilters(const pidProfile_t *pidProfile) {
 // 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, const rxConfig_t *rxConfig)
+         const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
 {
-    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], relaxFactor[3];
+    static float Kp[3], Ki[3], Kd[3], c[3];
-    float delta;
+    static float rollPitchMaxVelocity, yawMaxVelocity;
-    int axis;
+    static float previousSetpoint[3], relaxFactor[3];
-    float horizonLevelStrength = 1;
+    static float dT;
 
-    float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
+    if (!dT) {
+        dT = (float)targetPidLooptime * 0.000001f;
+    }
 
     pidInitFilters(pidProfile);
 
+    float horizonLevelStrength = 1;
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
-        const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
+        const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, midrc));
-        const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
+        const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, midrc));
         const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
         // Progressively turn off the horizon self level strength as the stick is banged over
         horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500;  // 1 at centre stick, 0 = max stick deflection
@@ -172,7 +155,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
     }
 
     // ----------PID controller----------
-    for (axis = 0; axis < 3; axis++) {
+    const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
+    for (int axis = 0; axis < 3; axis++) {
 
         static uint8_t configP[3], configI[3], configD[3];
 
@@ -184,8 +168,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
             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();
+            yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT;
-            rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
+            rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
 
             configP[axis] = pidProfile->P8[axis];
             configI[axis] = pidProfile->I8[axis];
@@ -222,16 +206,16 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
             }
         }
 
-        PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
+        const float PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
 
         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
         //  ---------- 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
+        const float errorRate = setpointRate[axis] - PVRate;       // r - y
 
         // -----calculate P component and add Dynamic Part based on stick input
-        PTerm = Kp[axis] * errorRate * tpaFactor;
+        float PTerm = Kp[axis] * errorRate * tpaFactor;
 
         // -----calculate I component.
         // Reduce strong Iterm accumulation during higher stick inputs
@@ -242,15 +226,16 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
         // limit maximum integrator value to prevent WindUp
         float itermScaler = setpointRateScaler * kiThrottleGain;
 
-        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f);
+        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * dT * itermScaler, -250.0f, 250.0f);
 
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
-        ITerm = errorGyroIf[axis];
+        const float ITerm = errorGyroIf[axis];
 
         //-----calculate D-term (Yaw D not yet supported)
+        float DTerm;
         if (axis != YAW) {
             static float previousSetpoint[3];
-            dynC = c[axis];
+            float dynC = c[axis];
             if (pidProfile->setpointRelaxRatio < 100) {
                 dynC = c[axis];
                 if (setpointRate[axis] > 0) {
@@ -262,12 +247,12 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
                 }
             }
             previousSetpoint[axis] = setpointRate[axis];
-            rD = dynC * setpointRate[axis] - PVRate;    // cr - y
+            const float rD = dynC * setpointRate[axis] - PVRate;    // cr - y
-            delta = rD - lastRateError[axis];
+            float delta = rD - lastRateError[axis];
             lastRateError[axis] = rD;
 
             // Divide delta by targetLooptime to get differential (ie dr/dt)
-            delta *= (1.0f / getdT());
+            delta *= (1.0f / dT);
 
             if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor;
 
@@ -278,7 +263,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
                 if (pidProfile->dterm_filter_type == FILTER_BIQUAD)
                     delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
                 else if (pidProfile->dterm_filter_type == FILTER_PT1)
-                    delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
+                    delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, dT);
                 else
                     delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta);
             }
@@ -288,7 +273,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
             // -----calculate total PID output
             axisPIDf[axis] = PTerm + ITerm + DTerm;
         } else {
-            if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
+            if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, dT);
 
             axisPIDf[axis] = PTerm + ITerm;
 

src/main/flight/pid.h

@@ -90,14 +90,9 @@ typedef struct pidProfile_s {
 #endif
 } pidProfile_t;
 
-struct controlRateConfig_s;
 union rollAndPitchTrims_u;
-struct rxConfig_s;
-typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
-        const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig);            // pid controller function prototype
-
 void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
-        const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig);
+        const union rollAndPitchTrims_u *angleTrim, uint16_t midrc);
 
 extern float axisPIDf[3];
 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];