OP-1474 Make it possible to select the combination of P, I, and D to scale

flight/libraries/math/mathmisc.h

@@ -88,7 +88,7 @@ typedef struct pointf {
 } pointf;
 
 // Returns the y value, given x, on the line passing through the points p0 and p1.
-static inline float y_on_line(float x, pointf *p0, pointf *p1)
+static inline float y_on_line(float x, const pointf *p0, const pointf *p1)
 {
     // Setup line y = m * x + b.
     const float dY1 = p1->y - p0->y;
@@ -102,7 +102,7 @@ static inline float y_on_line(float x, pointf *p0, pointf *p1)
 
 // Returns the y value, given x, on the curve defined by the points array.
 // The fist and last line of the curve extends beyond the first resp. last points.
-static inline float y_on_curve(float x, pointf points[], int num_points)
+static inline float y_on_curve(float x, const pointf points[], int num_points)
 {
     // Find the two points x is within.
     // If x is smaller than the first point's x value, use the first line of the curve.

flight/libraries/math/pid.c

@@ -76,12 +76,12 @@ float pid_apply(struct pid *pid, const float err, float dT)
  * This version of apply uses setpoint weighting for the derivative component so the gain
  * on the gyro derivative can be different than the gain on the setpoint derivative
  */
-float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT)
+float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT)
 {
     float err = setpoint - measured;
 
     // Scale up accumulator by 1000 while computing to avoid losing precision
-    pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
+    pid->iAccumulator += err * (scaler->i * pid->i * dT * 1000.0f);
     pid->iAccumulator  = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
 
     // Calculate DT1 term,
@@ -89,11 +89,11 @@ float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoi
     float diff  = ((deriv_gamma * setpoint - measured) - pid->lastErr);
     pid->lastErr = (deriv_gamma * setpoint - measured);
     if (pid->d > 0.0f && dT > 0.0f) {
-        dterm = pid->lastDer + dT / (dT + deriv_tau) * ((factor * diff * pid->d / dT) - pid->lastDer);
+        dterm = pid->lastDer + dT / (dT + deriv_tau) * ((scaler->d * diff * pid->d / dT) - pid->lastDer);
         pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
     } // 7.9577e-3  means 20 Hz f_cutoff
 
-    return (err * factor * pid->p) + pid->iAccumulator / 1000.0f + dterm;
+    return (err * scaler->p * pid->p) + pid->iAccumulator / 1000.0f + dterm;
 }
 
 /**
@@ -140,10 +140,3 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
     pid->d    = d;
     pid->iLim = iLim;
 }
-
-float pid_scale_factor(pid_scaler *scaler)
-{
-    float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0]));
-
-    return 1.0f + (IS_REAL(y) ? y : 0.0f);
-}

flight/libraries/math/pid.h

@@ -45,16 +45,16 @@ struct pid {
 };
 
 typedef struct pid_scaler {
-    float  x;
+    float p;
-    pointf points[5];
+    float i;
+    float d;
 } pid_scaler;
 
 // ! Methods to use the pid structures
 float pid_apply(struct pid *pid, const float err, float dT);
-float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT);
+float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT);
 void pid_zero(struct pid *pid);
 void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
 void pid_configure_derivative(float cutoff, float gamma);
-float pid_scale_factor(pid_scaler *scaler);
 
 #endif /* PID_H */

flight/modules/Stabilization/altitudeloop.c

@@ -164,7 +164,9 @@ static void altitudeHoldTask(void)
     switch (thrustMode) {
     case ALTITUDEHOLD:
         // altitude control loop
-        altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, thrustSetpoint, positionStateDown, dT);
+        // No scaling.
+        pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
+        altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT);
         break;
     case ALTITUDEVARIO:
         altitudeHoldStatus.VelocityDesired = thrustSetpoint;
@@ -182,7 +184,9 @@ static void altitudeHoldTask(void)
         break;
     default:
         // velocity control loop
-        thrustDemand = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
+        // No scaling.
+        pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
+        thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
 
         break;
     }

flight/modules/Stabilization/innerloop.c

@@ -123,29 +123,90 @@ static float get_pid_scale_source_value()
     return value;
 }
 
-static float get_pid_scale_factor()
+static int is_pid_thrust_scaled_for_axis(int axis)
-{
-    struct pid_scaler scaler = {
-        .x      = get_pid_scale_source_value(),
-        .points = {
-            { 0.0f,  stabSettings.stabBank.ThrustPIDScaleCurve[0] },
-            { 0.25f, stabSettings.stabBank.ThrustPIDScaleCurve[1] },
-            { 0.50f, stabSettings.stabBank.ThrustPIDScaleCurve[2] },
-            { 0.75f, stabSettings.stabBank.ThrustPIDScaleCurve[3] },
-            { 1.0f,  stabSettings.stabBank.ThrustPIDScaleCurve[4] }
-        }
-    };
-
-    return pid_scale_factor(&scaler);
-}
-
-static int is_pid_scaled_for_axis(int axis)
 {
     return stabSettings.stabBank.EnableThrustPIDScaling
            && (axis == 0 // Roll
                || axis == 1); // Pitch
 }
 
+static bool is_p_scaling_enabled()
+{
+    uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
+
+    return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P;
+}
+
+static bool is_i_scaling_enabled()
+{
+    uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
+
+    return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I;
+}
+
+static bool is_d_scaling_enabled()
+{
+    uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
+
+    return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
+}
+
+typedef struct pid_curve_scaler {
+    float  x;
+    pointf points[5];
+} pid_curve_scaler;
+
+static float pid_curve_value(const pid_curve_scaler *scaler)
+{
+    float y = y_on_curve(scaler->x, scaler->points, sizeof(scaler->points) / sizeof(scaler->points[0]));
+
+    return 1.0f + (IS_REAL(y) ? y : 0.0f);
+}
+
+static pid_scaler create_pid_scaler(int axis)
+{
+    pid_scaler scaler;
+
+    // Always scaled with the this.
+    scaler.p = scaler.i = scaler.d = speedScaleFactor;
+
+    if (is_pid_thrust_scaled_for_axis(axis)) {
+        const pid_curve_scaler curve_scaler = {
+            .x      = get_pid_scale_source_value(),
+            .points = {
+                { 0.00f, stabSettings.stabBank.ThrustPIDScaleCurve[0] },
+                { 0.25f, stabSettings.stabBank.ThrustPIDScaleCurve[1] },
+                { 0.50f, stabSettings.stabBank.ThrustPIDScaleCurve[2] },
+                { 0.75f, stabSettings.stabBank.ThrustPIDScaleCurve[3] },
+                { 1.00f, stabSettings.stabBank.ThrustPIDScaleCurve[4] }
+            }
+        };
+
+        float curve_value = pid_curve_value(&curve_scaler);
+
+        if (is_p_scaling_enabled()) {
+            scaler.p *= curve_value;
+        }
+        if (is_i_scaling_enabled()) {
+            scaler.i *= curve_value;
+        }
+        if (is_d_scaling_enabled()) {
+            scaler.d *= curve_value;
+        }
+    }
+
+    return scaler;
+}
+
 /**
  * WARNING! This callback executes with critical flight control priority every
  * time a gyroscope update happens do NOT put any time consuming calculations
@@ -252,11 +313,8 @@ static void stabilizationInnerloopTask()
                                  -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
                                  StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
                                  );
-                float scaleFactor = speedScaleFactor;
+                pid_scaler scaler = create_pid_scaler(t);
-                if (is_pid_scaled_for_axis(t)) {
+                actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT);
-                    scaleFactor *= get_pid_scale_factor();
-                }
-                actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], scaleFactor, rate[t], gyro_filtered[t], dT);
                 break;
             case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
             default:

shared/uavobjectdefinition/stabilizationbank.xml

@@ -18,6 +18,7 @@
 	<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
 	<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
 	<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
+	<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
 
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="false" updatemode="manual" period="0"/>

shared/uavobjectdefinition/stabilizationsettingsbank1.xml

@@ -18,6 +18,7 @@
 	<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
 	<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
 	<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
+	<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
 
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/stabilizationsettingsbank2.xml

@@ -18,6 +18,7 @@
 	<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
 	<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
 	<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
+	<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
 
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/stabilizationsettingsbank3.xml

@@ -18,6 +18,7 @@
 	<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
 	<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
 	<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
+	<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
 
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="true" updatemode="onchange" period="0"/>