Move PID gains to UINT16 instead of UINT8. Keep it compatible on the MSP level

docs/Programming Framework.md

@@ -80,6 +80,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 3             | FLIGHT_MODE   | `value` points to flight modes table                  |
 | 4             | LC            | `value` points to other logic condition ID            |
 | 5             | GVAR          | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
+| 5             | PID          | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
 
 #### FLIGHT
 

src/main/cms/cms_menu_imu.c

@@ -55,7 +55,7 @@
 #define RPY_PIDFF_MAX 200
 #define OTHER_PIDDF_MAX 255
 
-#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT8_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
+#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
 #define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX)
 #define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX)
 

src/main/fc/cli.c

@@ -44,6 +44,7 @@ extern uint8_t __config_end;
 #include "common/time.h"
 #include "common/typeconversion.h"
 #include "programming/global_variables.h"
+#include "programming/pid.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -2001,47 +2002,50 @@ static void cliGvar(char *cmdline) {
     }
 }
 
-static void printPid(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions)
+static void printPid(uint8_t dumpMask, const programmingPid_t *programmingPids, const programmingPid_t *defaultProgrammingPids)
 {
     const char *format = "pid %d %d %d %d %d %d %d %d %d %d";
-    for (uint32_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
+    for (uint32_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
-        const logicCondition_t logic = logicConditions[i];
+        const programmingPid_t pid = programmingPids[i];
 
         bool equalsDefault = false;
-        if (defaultLogicConditions) {
+        if (defaultProgrammingPids) {
-            logicCondition_t defaultValue = defaultLogicConditions[i];
+            programmingPid_t defaultValue = defaultProgrammingPids[i];
             equalsDefault =
-                logic.enabled == defaultValue.enabled &&
+                pid.enabled == defaultValue.enabled &&
-                logic.activatorId == defaultValue.activatorId &&
+                pid.setpoint.type == defaultValue.setpoint.type &&
-                logic.operation == defaultValue.operation &&
+                pid.setpoint.value == defaultValue.setpoint.value &&
-                logic.operandA.type == defaultValue.operandA.type &&
+                pid.measurement.type == defaultValue.measurement.type &&
-                logic.operandA.value == defaultValue.operandA.value &&
+                pid.measurement.value == defaultValue.measurement.value &&
-                logic.operandB.type == defaultValue.operandB.type &&
+                pid.gains.P == defaultValue.gains.P && 
-                logic.operandB.value == defaultValue.operandB.value &&
+                pid.gains.I == defaultValue.gains.I && 
-                logic.flags == defaultValue.flags;
+                pid.gains.D == defaultValue.gains.D && 
+                pid.gains.FF == defaultValue.gains.FF;
 
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
-                logic.enabled,
+                pid.enabled,
-                logic.activatorId,
+                pid.setpoint.type,
-                logic.operation,
+                pid.setpoint.value,
-                logic.operandA.type,
+                pid.measurement.type,
-                logic.operandA.value,
+                pid.measurement.value,
-                logic.operandB.type,
+                pid.gains.P,
-                logic.operandB.value,
+                pid.gains.I,
-                logic.flags
+                pid.gains.D,
+                pid.gains.FF
             );
         }
         cliDumpPrintLinef(dumpMask, equalsDefault, format,
             i,
-            logic.enabled,
+            pid.enabled,
-            logic.activatorId,
+            pid.setpoint.type,
-            logic.operation,
+            pid.setpoint.value,
-            logic.operandA.type,
+            pid.measurement.type,
-            logic.operandA.value,
+            pid.measurement.value,
-            logic.operandB.type,
+            pid.gains.P,
-            logic.operandB.value,
+            pid.gains.I,
-            logic.flags
+            pid.gains.D,
+            pid.gains.FF
         );
     }
 }
@@ -2052,7 +2056,7 @@ static void cliPid(char *cmdline) {
     uint8_t len = strlen(cmdline);
 
     if (len == 0) {
-        printLogic(DUMP_MASTER, programmingPids(0), NULL);
+        printPid(DUMP_MASTER, programmingPids(0), NULL);
     } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
         pgResetCopy(programmingPidsMutable(0), PG_LOGIC_CONDITIONS);
     } else {
@@ -2082,24 +2086,28 @@ static void cliPid(char *cmdline) {
 
         int32_t i = args[INDEX];
         if (
-            i >= 0 && i < MAX_LOGIC_CONDITIONS &&
+            i >= 0 && i < MAX_PROGRAMMING_PID_COUNT &&
             args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
-            args[ACTIVATOR_ID] >= -1 && args[ACTIVATOR_ID] < MAX_LOGIC_CONDITIONS &&
+            args[SETPOINT_TYPE] >= 0 && args[SETPOINT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
-            args[OPERATION] >= 0 && args[OPERATION] < LOGIC_CONDITION_LAST &&
+            args[SETPOINT_VALUE] >= -1000000 && args[SETPOINT_VALUE] <= 1000000 &&
-            args[OPERAND_A_TYPE] >= 0 && args[OPERAND_A_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[MEASUREMENT_TYPE] >= 0 && args[MEASUREMENT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
-            args[OPERAND_A_VALUE] >= -1000000 && args[OPERAND_A_VALUE] <= 1000000 &&
+            args[MEASUREMENT_VALUE] >= -1000000 && args[MEASUREMENT_VALUE] <= 1000000 &&
-            args[OPERAND_B_TYPE] >= 0 && args[OPERAND_B_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[P_GAIN] >= 0 && args[P_GAIN] <= INT16_MAX &&
-            args[OPERAND_B_VALUE] >= -1000000 && args[OPERAND_B_VALUE] <= 1000000 &&
+            args[I_GAIN] >= 0 && args[I_GAIN] <= INT16_MAX &&
-            args[FLAGS] >= 0 && args[FLAGS] <= 255
+            args[D_GAIN] >= 0 && args[D_GAIN] <= INT16_MAX &&
-
+            args[FF_GAIN] >= 0 && args[FF_GAIN] <= INT16_MAX
         ) {
             programmingPidsMutable(i)->enabled = args[ENABLED];
-            logicConditionsMutable(i)->setpoint.type = args[OPERAND_A_TYPE];
+            programmingPidsMutable(i)->setpoint.type = args[SETPOINT_TYPE];
-            logicConditionsMutable(i)->setpoint.value = args[OPERAND_A_VALUE];
+            programmingPidsMutable(i)->setpoint.value = args[SETPOINT_VALUE];
-            logicConditionsMutable(i)->measurement.type = args[OPERAND_B_TYPE];
+            programmingPidsMutable(i)->measurement.type = args[MEASUREMENT_TYPE];
-            logicConditionsMutable(i)->measurement.value = args[OPERAND_B_VALUE];
+            programmingPidsMutable(i)->measurement.value = args[MEASUREMENT_VALUE];
+            programmingPidsMutable(i)->gains.P = args[P_GAIN];
+            programmingPidsMutable(i)->gains.I = args[I_GAIN];
+            programmingPidsMutable(i)->gains.D = args[D_GAIN];
+            programmingPidsMutable(i)->gains.FF = args[FF_GAIN];
 
-            cliLogic("");
+            cliPid("");
         } else {
             cliShowParseError();
         }
@@ -3422,7 +3430,7 @@ static void printConfig(const char *cmdline, bool doDiff)
         printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
 
         cliPrintHashLine("pid");
-        printLogic(dumpMask, programmingPids_CopyArray, programmingPids(0));
+        printPid(dumpMask, programmingPids_CopyArray, programmingPids(0));
 #endif
 
         cliPrintHashLine("feature");

src/main/fc/fc_core.c

@@ -89,6 +89,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/failsafe.h"
 
 #include "config/feature.h"
+#include "programming/pid.h"
 
 // June 2013     V2.2-dev
 
@@ -390,6 +391,7 @@ void disarm(disarmReason_t disarmReason)
 
         statsOnDisarm();
         logicConditionReset();
+        programmingPidReset();
         beeper(BEEPER_DISARMING);      // emit disarm tone
     }
 }
@@ -480,6 +482,7 @@ void tryArm(void)
         //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
         ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
         logicConditionReset();
+        programmingPidReset();
         headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
         resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));

src/main/fc/fc_msp.c

@@ -686,18 +686,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 
     case MSP_PID:
         for (int i = 0; i < PID_ITEM_COUNT; i++) {
-            sbufWriteU8(dst, pidBank()->pid[i].P);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
-            sbufWriteU8(dst, pidBank()->pid[i].I);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
-            sbufWriteU8(dst, pidBank()->pid[i].D);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
         }
         break;
 
     case MSP2_PID:
         for (int i = 0; i < PID_ITEM_COUNT; i++) {
-            sbufWriteU8(dst, pidBank()->pid[i].P);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
-            sbufWriteU8(dst, pidBank()->pid[i].I);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
-            sbufWriteU8(dst, pidBank()->pid[i].D);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
-            sbufWriteU8(dst, pidBank()->pid[i].FF);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255));
         }
         break;
 

src/main/flight/pid.h

@@ -81,10 +81,10 @@ typedef enum {
 } pidType_e;
 
 typedef struct pid8_s {
-    uint8_t P;
+    uint16_t P;
-    uint8_t I;
+    uint16_t I;
-    uint8_t D;
+    uint16_t D;
-    uint8_t FF;
+    uint16_t FF;
 } pid8_t;
 
 typedef struct pidBank_s {

src/main/navigation/navigation.c

@@ -1962,6 +1962,10 @@ float navPidApply3(
         }
     }
     
+    if (pidFlags & PID_LIMIT_INTEGRATOR) {
+        pid->integrator = constrainf(pid->integrator, outMin, outMax);
+    } 
+
     return outValConstrained;
 }
 

src/main/navigation/navigation_private.h

@@ -95,6 +95,7 @@ typedef enum {
     PID_DTERM_FROM_ERROR            = 1 << 0,
     PID_ZERO_INTEGRATOR             = 1 << 1,
     PID_SHRINK_INTEGRATOR           = 1 << 2,
+    PID_LIMIT_INTEGRATOR            = 1 << 3,
 } pidControllerFlags_e;
 
 typedef struct {

src/main/programming/logic_condition.c

@@ -30,6 +30,7 @@
 
 #include "programming/logic_condition.h"
 #include "programming/global_variables.h"
+#include "programming/pid.h"
 #include "common/utils.h"
 #include "rx/rx.h"
 #include "common/maths.h"
@@ -595,6 +596,12 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
             }
             break;
 
+        case LOGIC_CONDITION_OPERAND_TYPE_PID:
+            if (operand >= 0 && operand < MAX_PROGRAMMING_PID_COUNT) {
+                retVal = programmingPidGetOutput(operand);
+            }
+            break;
+
         default:
             break;
     }

src/main/programming/logic_condition.h

@@ -77,6 +77,7 @@ typedef enum logicOperandType_s {
     LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE,
     LOGIC_CONDITION_OPERAND_TYPE_LC,    // Result of different LC and LC operand
     LOGIC_CONDITION_OPERAND_TYPE_GVAR,  // Value from a global variable
+    LOGIC_CONDITION_OPERAND_TYPE_PID,  // Value from a Programming PID
     LOGIC_CONDITION_OPERAND_TYPE_LAST
 } logicOperandType_e;
 

src/main/programming/pid.c

@@ -35,6 +35,7 @@ FILE_COMPILE_FOR_SIZE
 #include "navigation/navigation_private.h"
 
 #include "programming/pid.h"
+#include "programming/logic_condition.h"
 
 EXTENDED_FASTRAM programmingPidState_t programmingPidState[MAX_PROGRAMMING_PID_COUNT];
 static bool pidsInitiated = false;
@@ -66,13 +67,31 @@ void pgResetFn_programmingPids(programmingPid_t *instance)
 
 void programmingPidUpdateTask(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
+    static timeUs_t previousUpdateTimeUs;
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
     
     if (!pidsInitiated) {
         programmingPidInit();
         pidsInitiated = true;
     }
 
+    for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        if (programmingPids(i)->enabled) {
+            const int setpoint = logicConditionGetOperandValue(programmingPids(i)->setpoint.type, programmingPids(i)->setpoint.value);
+            const int measurement = logicConditionGetOperandValue(programmingPids(i)->measurement.type, programmingPids(i)->measurement.value);
+
+            programmingPidState[i].output = navPidApply2(
+                &programmingPidState[i].controller,
+                setpoint,
+                measurement,
+                dT,
+                -1000,
+                1000,
+                PID_LIMIT_INTEGRATOR
+            );
+
+        }
+    }
 }
 
 void programmingPidInit(void)
@@ -80,13 +99,24 @@ void programmingPidInit(void)
     for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
         navPidInit(
             &programmingPidState[i].controller,
-            programmingPids(i)->gains.P / 100.0f,
+            programmingPids(i)->gains.P / 1000.0f,
-            programmingPids(i)->gains.I / 100.0f,
+            programmingPids(i)->gains.I / 1000.0f,
-            programmingPids(i)->gains.D / 100.0f,
+            programmingPids(i)->gains.D / 1000.0f,
-            programmingPids(i)->gains.FF / 100.0f,
+            programmingPids(i)->gains.FF / 1000.0f,
             5.0f
         );
     }
 }
 
+int programmingPidGetOutput(uint8_t i) {
+    return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output;
+}
+
+void programmingPidReset(void)
+{
+    for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        navPidReset(&programmingPidState[i].controller);
+    }
+}
+
 #endif

src/main/programming/pid.h

@@ -32,7 +32,7 @@
 #include "flight/pid.h"
 #include "navigation/navigation.h"
 
-#define MAX_PROGRAMMING_PID_COUNT 2
+#define MAX_PROGRAMMING_PID_COUNT 4
 
 typedef struct programmingPid_s {
     uint8_t enabled;
@@ -45,8 +45,10 @@ PG_DECLARE_ARRAY(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids);
 
 typedef struct programmingPidState_s {
     pidController_t controller;
-    logicOperand_t setpoint;
+    float output;
 } programmingPidState_t;
 
 void programmingPidUpdateTask(timeUs_t currentTimeUs);
 void programmingPidInit(void);
+void programmingPidReset(void);
+int programmingPidGetOutput(uint8_t i);