Add user programmable PID controllers to a Programming Framework

docs/Programming Framework.md

@@ -9,10 +9,13 @@ INAV Programming Framework coinsists of:
 
 * Logic Conditions - each Logic Condition can be understood as a single command, a single line of code
 * Global Variables - variables that can store values from and for LogiC Conditions and servo mixer
+* Programming PID - general purpose, user configurable PID controllers
 
 IPF can be edited using INAV Configurator user interface, of via CLI
 
-## CLI
+## Logic Conditions
+
+### CLI
 
 `logic <rule> <enabled> <activatorId> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>`
 
@@ -80,6 +83,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
 
@@ -120,7 +124,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 32            | CROSSFIRE LQ          | Crossfire Link quality as returned by the CRSF protocol   | 
 | 33            | CROSSFIRE SNR          | Crossfire SNR as returned by the CRSF protocol   | 
 
-##### ACTIVE_WAYPOINT_ACTION
+#### ACTIVE_WAYPOINT_ACTION
 
 | Action        |  Value   |
 |----           |----      |
@@ -158,6 +162,27 @@ All flags are reseted on ARM and DISARM event.
 |----   |----       |----                   |
 | 0     | 1         | Latch - after activation LC will stay active until LATCH flag is reseted |
 
+## Global variables
+
+### CLI
+
+`gvar <index> <default value> <min> <max>`
+
+## Programming PID
+
+`pid <index> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>`
+
+* `<index>` - ID of PID Controller, starting from `0`
+* `<enabled>` - `0` evaluates as disabled, `1` evaluates as enabled
+* `<setpoint type>` - See `Operands` paragraph
+* `<setpoint value>` - See `Operands` paragraph
+* `<measurement type>` - See `Operands` paragraph
+* `<measurement value>` - See `Operands` paragraph
+* `<P gain>` - P-gain, scaled to `1/1000`
+* `<I gain>` - I-gain, scaled to `1/1000`
+* `<D gain>` - D-gain, scaled to `1/1000`
+* `<FF gain>` - FF-gain, scaled to `1/1000`
+
 ## Examples
 
 ### Dynamic THROTTLE scale

docs/Settings.md

@@ -324,7 +324,7 @@
 | nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
 | nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
 | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
-| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
+| nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
 | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
 | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
 | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. |

src/main/CMakeLists.txt

@@ -362,6 +362,8 @@ main_sources(COMMON_SRC
     programming/global_variables.h
     programming/programming_task.c
     programming/programming_task.h
+    programming/pid.c
+    programming/pid.h
 
     rx/crsf.c
     rx/crsf.h

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/config/parameter_group_ids.h

@@ -115,7 +115,8 @@
 #define PG_OSD_LAYOUTS_CONFIG 1025
 #define PG_SAFE_HOME_CONFIG 1026
 #define PG_DJI_OSD_CONFIG 1027
-#define PG_INAV_END 1027
+#define PG_PROGRAMMING_PID 1028
+#define PG_INAV_END 1028
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/cli.c

@@ -42,6 +42,7 @@ uint8_t cliMode = 0;
 #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"
@@ -1998,6 +1999,118 @@ static void cliGvar(char *cmdline) {
     }
 }
 
+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_PROGRAMMING_PID_COUNT; i++) {
+        const programmingPid_t pid = programmingPids[i];
+
+        bool equalsDefault = false;
+        if (defaultProgrammingPids) {
+            programmingPid_t defaultValue = defaultProgrammingPids[i];
+            equalsDefault =
+                pid.enabled == defaultValue.enabled &&
+                pid.setpoint.type == defaultValue.setpoint.type &&
+                pid.setpoint.value == defaultValue.setpoint.value &&
+                pid.measurement.type == defaultValue.measurement.type &&
+                pid.measurement.value == defaultValue.measurement.value &&
+                pid.gains.P == defaultValue.gains.P && 
+                pid.gains.I == defaultValue.gains.I && 
+                pid.gains.D == defaultValue.gains.D && 
+                pid.gains.FF == defaultValue.gains.FF;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format,
+                i,
+                pid.enabled,
+                pid.setpoint.type,
+                pid.setpoint.value,
+                pid.measurement.type,
+                pid.measurement.value,
+                pid.gains.P,
+                pid.gains.I,
+                pid.gains.D,
+                pid.gains.FF
+            );
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format,
+            i,
+            pid.enabled,
+            pid.setpoint.type,
+            pid.setpoint.value,
+            pid.measurement.type,
+            pid.measurement.value,
+            pid.gains.P,
+            pid.gains.I,
+            pid.gains.D,
+            pid.gains.FF
+        );
+    }
+}
+
+static void cliPid(char *cmdline) {
+    char * saveptr;
+    int args[10], check = 0;
+    uint8_t len = strlen(cmdline);
+
+    if (len == 0) {
+        printPid(DUMP_MASTER, programmingPids(0), NULL);
+    } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
+        pgResetCopy(programmingPidsMutable(0), PG_LOGIC_CONDITIONS);
+    } else {
+        enum {
+            INDEX = 0,
+            ENABLED,
+            SETPOINT_TYPE,
+            SETPOINT_VALUE,
+            MEASUREMENT_TYPE,
+            MEASUREMENT_VALUE,
+            P_GAIN,
+            I_GAIN,
+            D_GAIN,
+            FF_GAIN,
+            ARGS_COUNT
+            };
+        char *ptr = strtok_r(cmdline, " ", &saveptr);
+        while (ptr != NULL && check < ARGS_COUNT) {
+            args[check++] = fastA2I(ptr);
+            ptr = strtok_r(NULL, " ", &saveptr);
+        }
+
+        if (ptr != NULL || check != ARGS_COUNT) {
+            cliShowParseError();
+            return;
+        }
+
+        int32_t i = args[INDEX];
+        if (
+            i >= 0 && i < MAX_PROGRAMMING_PID_COUNT &&
+            args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
+            args[SETPOINT_TYPE] >= 0 && args[SETPOINT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[SETPOINT_VALUE] >= -1000000 && args[SETPOINT_VALUE] <= 1000000 &&
+            args[MEASUREMENT_TYPE] >= 0 && args[MEASUREMENT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[MEASUREMENT_VALUE] >= -1000000 && args[MEASUREMENT_VALUE] <= 1000000 &&
+            args[P_GAIN] >= 0 && args[P_GAIN] <= INT16_MAX &&
+            args[I_GAIN] >= 0 && args[I_GAIN] <= INT16_MAX &&
+            args[D_GAIN] >= 0 && args[D_GAIN] <= INT16_MAX &&
+            args[FF_GAIN] >= 0 && args[FF_GAIN] <= INT16_MAX
+        ) {
+            programmingPidsMutable(i)->enabled = args[ENABLED];
+            programmingPidsMutable(i)->setpoint.type = args[SETPOINT_TYPE];
+            programmingPidsMutable(i)->setpoint.value = args[SETPOINT_VALUE];
+            programmingPidsMutable(i)->measurement.type = args[MEASUREMENT_TYPE];
+            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];
+
+            cliPid("");
+        } else {
+            cliShowParseError();
+        }
+    }
+}
+
 #endif
 
 #ifdef USE_SDCARD
@@ -3312,6 +3425,9 @@ static void printConfig(const char *cmdline, bool doDiff)
 
         cliPrintHashLine("gvar");
         printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
+
+        cliPrintHashLine("pid");
+        printPid(dumpMask, programmingPids_CopyArray, programmingPids(0));
 #endif
 
         cliPrintHashLine("feature");
@@ -3566,6 +3682,10 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("gvar", "configure global variables",
         "<gvar> <default> <min> <max>\r\n"
         "\treset\r\n", cliGvar),
+
+    CLI_COMMAND_DEF("pid", "configurable PID controllers",
+        "<#> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>\r\n"
+        "\treset\r\n", cliPid),
 #endif
     CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
     CLI_COMMAND_DEF("smix", "servo mixer",

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

@@ -36,6 +36,7 @@
 #include "common/time.h"
 #include "common/utils.h"
 #include "programming/global_variables.h"
+#include "programming/pid.h"
 
 #include "config/parameter_group_ids.h"
 
@@ -553,6 +554,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU32(dst, gvGet(i));
         }
         break;
+    case MSP2_INAV_PROGRAMMING_PID:
+        for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+            sbufWriteU8(dst, programmingPids(i)->enabled);
+            sbufWriteU8(dst, programmingPids(i)->setpoint.type);
+            sbufWriteU32(dst, programmingPids(i)->setpoint.value);
+            sbufWriteU8(dst, programmingPids(i)->measurement.type);
+            sbufWriteU32(dst, programmingPids(i)->measurement.value);
+            sbufWriteU16(dst, programmingPids(i)->gains.P);
+            sbufWriteU16(dst, programmingPids(i)->gains.I);
+            sbufWriteU16(dst, programmingPids(i)->gains.D);
+            sbufWriteU16(dst, programmingPids(i)->gains.FF);
+        }
+        break;
 #endif
     case MSP2_COMMON_MOTOR_MIXER:
         for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@@ -686,18 +700,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, pidBank()->pid[i].I);
-            sbufWriteU8(dst, pidBank()->pid[i].D);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
+            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, pidBank()->pid[i].I);
-            sbufWriteU8(dst, pidBank()->pid[i].D);
-            sbufWriteU8(dst, pidBank()->pid[i].FF);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255));
         }
         break;
 
@@ -1969,6 +1983,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         } else
             return MSP_RESULT_ERROR;
         break;
+
+    case MSP2_INAV_SET_PROGRAMMING_PID:
+        sbufReadU8Safe(&tmp_u8, src);
+        if ((dataSize == 20) && (tmp_u8 < MAX_PROGRAMMING_PID_COUNT)) {
+            programmingPidsMutable(tmp_u8)->enabled = sbufReadU8(src);
+            programmingPidsMutable(tmp_u8)->setpoint.type = sbufReadU8(src);
+            programmingPidsMutable(tmp_u8)->setpoint.value = sbufReadU32(src);
+            programmingPidsMutable(tmp_u8)->measurement.type = sbufReadU8(src);
+            programmingPidsMutable(tmp_u8)->measurement.value = sbufReadU32(src);
+            programmingPidsMutable(tmp_u8)->gains.P = sbufReadU16(src);
+            programmingPidsMutable(tmp_u8)->gains.I = sbufReadU16(src);
+            programmingPidsMutable(tmp_u8)->gains.D = sbufReadU16(src);
+            programmingPidsMutable(tmp_u8)->gains.FF = sbufReadU16(src);
+        } else
+            return MSP_RESULT_ERROR;
+        break;
 #endif
     case MSP2_COMMON_SET_MOTOR_MIXER:
         sbufReadU8Safe(&tmp_u8, src);

src/main/fc/rc_adjustments.c

@@ -360,9 +360,9 @@ static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, u
     return applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_MANUAL_RATE_MIN, RC_ADJUSTMENT_MANUAL_RATE_MAX);
 }
 
-static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
+static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta)
 {
-    applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
+    applyAdjustmentU16(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
 }
 
 static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)

src/main/flight/pid.c

@@ -1116,7 +1116,7 @@ pidBank_t * pidBankMutable(void) {
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }
 
-uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
+uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
 {
     if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) {
        return &pidBank->pid[pidIndex].FF;

src/main/flight/pid.h

@@ -81,10 +81,10 @@ typedef enum {
 } pidType_e;
 
 typedef struct pid8_s {
-    uint8_t P;
-    uint8_t I;
-    uint8_t D;
-    uint8_t FF;
+    uint16_t P;
+    uint16_t I;
+    uint16_t D;
+    uint16_t FF;
 } pid8_t;
 
 typedef struct pidBank_s {
@@ -199,4 +199,4 @@ void autotuneUpdateState(void);
 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
 
 pidType_e pidIndexGetType(pidIndex_e pidIndex);
-uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);
+uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);

src/main/msp/msp_protocol_v2_inav.h

@@ -61,6 +61,8 @@
 #define MSP2_INAV_SET_GLOBAL_FUNCTIONS          0x2025
 #define MSP2_INAV_LOGIC_CONDITIONS_STATUS       0x2026
 #define MSP2_INAV_GVAR_STATUS                   0x2027
+#define MSP2_INAV_PROGRAMMING_PID               0x2028
+#define MSP2_INAV_SET_PROGRAMMING_PID           0x2029
 
 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031