Merge branch 'skarlsso/OP-1474-PID_Scaling_one_factor' of ssh://git.openpilot.org/OpenPilot into thread/OP-1474_PID_Scaling_Banks

flight/modules/Stabilization/inc/stabilization.h

@@ -59,6 +59,8 @@ typedef struct {
     }     monitor;
     float rattitude_mode_transition_stick_position;
     struct pid innerPids[3], outerPids[3];
+    // TPS [Roll,Pitch,Yaw][P,I,D]
+    bool  thrust_pid_scaling_enabled[3][3];
 } StabilizationData;
 
 

flight/modules/Stabilization/innerloop.c

@@ -123,43 +123,6 @@ static float get_pid_scale_source_value()
     return value;
 }
 
-static int is_pid_thrust_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];
@@ -179,7 +142,9 @@ static pid_scaler create_pid_scaler(int axis)
     // Always scaled with the this.
     scaler.p = scaler.i = scaler.d = speedScaleFactor;
 
-    if (is_pid_thrust_scaled_for_axis(axis)) {
+    if (stabSettings.thrust_pid_scaling_enabled[axis][0]
+        || stabSettings.thrust_pid_scaling_enabled[axis][1]
+        || stabSettings.thrust_pid_scaling_enabled[axis][2]) {
         const pid_curve_scaler curve_scaler = {
             .x      = get_pid_scale_source_value(),
             .points = {
@@ -193,13 +158,13 @@ static pid_scaler create_pid_scaler(int axis)
 
         float curve_value = pid_curve_value(&curve_scaler);
 
-        if (is_p_scaling_enabled()) {
+        if (stabSettings.thrust_pid_scaling_enabled[axis][0]) {
             scaler.p *= curve_value;
         }
-        if (is_i_scaling_enabled()) {
+        if (stabSettings.thrust_pid_scaling_enabled[axis][1]) {
             scaler.i *= curve_value;
         }
-        if (is_d_scaling_enabled()) {
+        if (stabSettings.thrust_pid_scaling_enabled[axis][2]) {
             scaler.d *= curve_value;
         }
     }

flight/modules/Stabilization/stabilization.c

@@ -229,6 +229,66 @@ static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     StabilizationBankSet(&stabSettings.stabBank);
 }
 
+static bool use_tps_for_roll()
+{
+    uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
+
+    return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
+           axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
+           axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
+           axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL;
+}
+
+static bool use_tps_for_pitch()
+{
+    uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
+
+    return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
+           axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
+           axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
+           axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH;
+}
+
+static bool use_tps_for_yaw()
+{
+    uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
+
+    return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
+           axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
+           axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
+           axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW;
+}
+
+static bool use_tps_for_p()
+{
+    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 use_tps_for_i()
+{
+    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 use_tps_for_d()
+{
+    uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
+
+    return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
+           target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
+}
+
 static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     StabilizationBankGet(&stabSettings.stabBank);
@@ -268,6 +328,24 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
                   stabSettings.stabBank.YawPI.Ki,
                   0,
                   stabSettings.stabBank.YawPI.ILimit);
+
+    bool tps_for_axis[3] = {
+        use_tps_for_roll(),
+        use_tps_for_pitch(),
+        use_tps_for_yaw()
+    };
+    bool tps_for_pid[3] = {
+        use_tps_for_p(),
+        use_tps_for_i(),
+        use_tps_for_d()
+    };
+    for (int axis = 0; axis < 3; axis++) {
+        for (int pid = 0; pid < 3; pid++) {
+            stabSettings.thrust_pid_scaling_enabled[axis][pid] = stabSettings.stabBank.EnableThrustPIDScaling
+                                                                 && tps_for_axis[axis]
+                                                                 && tps_for_pid[pid];
+        }
+    }
 }
 
 

shared/uavobjectdefinition/stabilizationbank.xml

@@ -19,6 +19,7 @@
 	<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" />
+	<field name="ThrustPIDScaleAxes" units="" type="enum" elements="1" options="Roll Pitch Yaw,Roll Pitch,Roll Yaw,Roll,Pitch Yaw,Pitch,Yaw" defaultvalue="Roll Pitch" />
 
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="false" updatemode="manual" period="0"/>

shared/uavobjectdefinition/stabilizationsettingsbank1.xml

@@ -19,6 +19,7 @@
 	<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" />
+	<field name="ThrustPIDScaleAxes" units="" type="enum" elements="1" options="Roll Pitch Yaw,Roll Pitch,Roll Yaw,Roll,Pitch Yaw,Pitch,Yaw" defaultvalue="Roll Pitch" />
 
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/stabilizationsettingsbank2.xml

@@ -19,6 +19,7 @@
 	<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" />
+	<field name="ThrustPIDScaleAxes" units="" type="enum" elements="1" options="Roll Pitch Yaw,Roll Pitch,Roll Yaw,Roll,Pitch Yaw,Pitch,Yaw" defaultvalue="Roll Pitch" />
 
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/stabilizationsettingsbank3.xml

@@ -19,6 +19,7 @@
 	<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" />
+	<field name="ThrustPIDScaleAxes" units="" type="enum" elements="1" options="Roll Pitch Yaw,Roll Pitch,Roll Yaw,Roll,Pitch Yaw,Pitch,Yaw" defaultvalue="Roll Pitch" />
 
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="true" updatemode="onchange" period="0"/>