Convert XY VEL TO ACC controller to use feed forward component (#4326)

* modify navPidApply3 to use linear FF component

* store FF component in BB log

* Make VEL_XY FF component configurable

* change scaling for PID_VEL_XY FF component

* MSP2_PID frame to setup FF component

* Set default VEL XY FF to 40

src/main/blackbox/blackbox.c

@@ -217,6 +217,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"mcVelAxisD",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
     {"mcVelAxisD",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
     {"mcVelAxisD",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisFF", 0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisFF", 1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
+    {"mcVelAxisFF", 2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
     {"mcVelAxisOut",0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
     {"mcVelAxisOut",1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
     {"mcVelAxisOut",2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
@@ -396,7 +399,7 @@ typedef struct blackboxMainState_s {
     int32_t axisPID_Setpoint[XYZ_AXIS_COUNT];
 
     int32_t mcPosAxisP[XYZ_AXIS_COUNT];
-    int32_t mcVelAxisPID[3][XYZ_AXIS_COUNT];
+    int32_t mcVelAxisPID[4][XYZ_AXIS_COUNT];
     int32_t mcVelAxisOutput[XYZ_AXIS_COUNT];
 
     int32_t mcSurfacePID[3];
@@ -706,7 +709,7 @@ static void writeIntraframe(void)
 
         blackboxWriteSignedVBArray(blackboxCurrent->mcPosAxisP, XYZ_AXIS_COUNT);
 
-        for (int i = 0; i < 3; i++) {
+        for (int i = 0; i < 4; i++) {
             blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisPID[i], XYZ_AXIS_COUNT);
         }
 
@@ -906,7 +909,7 @@ static void writeInterframe(void)
         arraySubInt32(deltas, blackboxCurrent->mcPosAxisP, blackboxLast->mcPosAxisP, XYZ_AXIS_COUNT);
         blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
 
-        for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
+        for (int i = 0; i < 4; i++) {
             arraySubInt32(deltas, blackboxCurrent->mcVelAxisPID[i], blackboxLast->mcVelAxisPID[i], XYZ_AXIS_COUNT);
             blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
         }
@@ -1338,6 +1341,7 @@ static void loadMainState(timeUs_t currentTimeUs)
             blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional);
             blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral);
             blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative);
+            blackboxCurrent->mcVelAxisPID[3][i] = lrintf(nav_pids->vel[i].feedForward);
             blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
         }
 #endif

src/main/fc/fc_msp.c

@@ -627,6 +627,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         }
         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);
+        }
+        break;
+
     case MSP_PIDNAMES:
         for (const char *c = pidnames; *c; c++) {
             sbufWriteU8(dst, *c);
@@ -1571,6 +1580,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             return MSP_RESULT_ERROR;
         break;
 
+    case MSP2_SET_PID:
+        if (dataSize >= PID_ITEM_COUNT * 4) {
+            for (int i = 0; i < PID_ITEM_COUNT; i++) {
+                pidBankMutable()->pid[i].P = sbufReadU8(src);
+                pidBankMutable()->pid[i].I = sbufReadU8(src);
+                pidBankMutable()->pid[i].D = sbufReadU8(src);
+                pidBankMutable()->pid[i].FF = sbufReadU8(src);
+            }
+            schedulePidGainsUpdate();
+#if defined(USE_NAV)
+            navigationUsePIDs();
+#endif
+        } else
+            return MSP_RESULT_ERROR;
+        break;
+
     case MSP_SET_MODE_RANGE:
         sbufReadU8Safe(&tmp_u8, src);
         if ((dataSize >= 5) && (tmp_u8 < MAX_MODE_ACTIVATION_CONDITION_COUNT)) {

src/main/fc/settings.yaml

@@ -1073,6 +1073,11 @@ groups:
         condition: USE_NAV
         min: 0
         max: 255
+      - name: nav_mc_vel_xy_ff
+        field: bank_mc.pid[PID_VEL_XY].FF
+        condition: USE_NAV
+        min: 0
+        max: 255
       - name: nav_mc_heading_p
         field: bank_mc.pid[PID_HEADING].P
         condition: USE_NAV

src/main/flight/pid.c

@@ -114,58 +114,66 @@ PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
             .pid = {
-                [PID_ROLL] =    { 40, 30, 23 },
+                [PID_ROLL] =    { 40, 30, 23, 0 },
-                [PID_PITCH] =   { 40, 30, 23 },
+                [PID_PITCH] =   { 40, 30, 23, 0 },
-                [PID_YAW] =     { 85, 45, 0 },
+                [PID_YAW] =     { 85, 45, 0, 0 },
                 [PID_LEVEL] = {
                     .P = 20,    // Self-level strength
                     .I = 15,    // Self-leveing low-pass frequency (0 - disabled)
                     .D = 75,    // 75% horizon strength
+                    .FF = 0,
                 },
-                [PID_HEADING] = { 60, 0, 0 },
+                [PID_HEADING] = { 60, 0, 0, 0 },
                 [PID_POS_XY] = {
                     .P = 65,   // NAV_POS_XY_P * 100
                     .I = 120,  // posDecelerationTime * 100
                     .D = 10,   // posResponseExpo * 100
+                    .FF = 0,
                 },
                 [PID_VEL_XY] = {
                     .P = 40,   // NAV_VEL_XY_P * 20
                     .I = 15,   // NAV_VEL_XY_I * 100
                     .D = 100,  // NAV_VEL_XY_D * 100
+                    .FF = 40,  // NAV_VEL_XY_D * 100
                 },
                 [PID_POS_Z] = {
                     .P = 50,    // NAV_POS_Z_P * 100
                     .I = 0,     // not used
                     .D = 0,     // not used
+                    .FF = 0,
                 },
                 [PID_VEL_Z] = {
                     .P = 100,   // NAV_VEL_Z_P * 66.7
                     .I = 50,    // NAV_VEL_Z_I * 20
                     .D = 10,    // NAV_VEL_Z_D * 100
+                    .FF = 0,
                 }
             }
         },
 
         .bank_fw = {
             .pid = {
-                [PID_ROLL] =    { 5, 7, 50 },
+                [PID_ROLL] =    { 5, 7, 50, 0 },
-                [PID_PITCH] =   { 5, 7, 50 },
+                [PID_PITCH] =   { 5, 7, 50, 0 },
-                [PID_YAW] =     { 6, 10, 60 },
+                [PID_YAW] =     { 6, 10, 60, 0 },
                 [PID_LEVEL] = {
                     .P = 20,    // Self-level strength
                     .I = 5,     // Self-leveing low-pass frequency (0 - disabled)
                     .D = 75,    // 75% horizon strength
+                    .FF = 0,
                 },
-                [PID_HEADING] = { 60, 0, 0 },
+                [PID_HEADING] = { 60, 0, 0, 0 },
                 [PID_POS_Z] = {
                     .P = 40,    // FW_POS_Z_P * 10
                     .I = 5,     // FW_POS_Z_I * 10
                     .D = 10,    // FW_POS_Z_D * 10
+                    .FF = 0,
                 },
                 [PID_POS_XY] = {
                     .P = 75,    // FW_POS_XY_P * 100
                     .I = 5,     // FW_POS_XY_I * 100
                     .D = 8,     // FW_POS_XY_D * 100
+                    .FF = 0,
                 }
             }
         },

src/main/flight/pid.h

@@ -69,6 +69,7 @@ typedef struct pid8_s {
     uint8_t P;
     uint8_t I;
     uint8_t D;
+    uint8_t FF;
 } pid8_t;
 
 typedef struct pidBank_s {

src/main/msp/msp_protocol_v2_inav.h

@@ -57,3 +57,6 @@
 #define MSP2_INAV_SET_SERVO_MIXER               0x2021
 #define MSP2_INAV_LOGIC_CONDITIONS              0x2022
 #define MSP2_INAV_SET_LOGIC_CONDITIONS          0x2023
+
+#define MSP2_PID                                0x2030
+#define MSP2_SET_PID                            0x2031

src/main/navigation/navigation.c

@@ -1632,7 +1632,7 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
 // http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
 float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler)
 {
-    float newProportional, newDerivative;
+    float newProportional, newDerivative, newFeedForward;
     float error = setpoint - measurement;
 
     /* P-term */
@@ -1660,13 +1660,19 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
         pid->integrator = 0.0f;
     }
 
+    /*
+     * Compute FeedForward parameter
+     */
+    newFeedForward = setpoint * pid->param.kFF * gainScaler;
+
     /* Pre-calculate output and limit it if actuator is saturating */
-    const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative;
+    const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
     const float outValConstrained = constrainf(outVal, outMin, outMax);
 
     pid->proportional = newProportional;
     pid->integral = pid->integrator;
     pid->derivative = newDerivative;
+    pid->feedForward = newFeedForward;
     pid->output_constrained = outValConstrained;
 
     /* Update I-term */
@@ -1701,16 +1707,18 @@ void navPidReset(pidController_t *pid)
     pid->derivative = 0.0f;
     pid->integrator = 0.0f;
     pid->last_input = 0.0f;
+    pid->feedForward = 0.0f;
     pid->dterm_filter_state.state = 0.0f;
     pid->dterm_filter_state.RC = 0.0f;
     pid->output_constrained = 0.0f;
 }
 
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD)
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF)
 {
     pid->param.kP = _kP;
     pid->param.kI = _kI;
     pid->param.kD = _kD;
+    pid->param.kFF = _kFF;
 
     if (_kI > 1e-6f && _kP > 1e-6f) {
         float Ti = _kP / _kI;
@@ -3028,7 +3036,9 @@ void navigationUsePIDs(void)
 
         navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
-                                               (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f);
+                                               (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
+                                               (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f
+        );
     }
 
     // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
@@ -3036,10 +3046,12 @@ void navigationUsePIDs(void)
 
     navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
                                         (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
-                                        (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f);
+                                        (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
+                                        0.0f);
 
     // Initialize surface tracking PID
     navPidInit(&posControl.pids.surface, 2.0f,
+                                         0.0f,
                                          0.0f,
                                          0.0f);
 
@@ -3047,11 +3059,13 @@ void navigationUsePIDs(void)
     // Initialize fixed wing PID controllers
     navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f);
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
+                                        0.0f);
 
     navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f);
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
+                                        0.0f);
 }
 
 void navigationInit(void)

src/main/navigation/navigation.h

@@ -240,6 +240,7 @@ typedef struct {
     float kI;
     float kD;
     float kT;   // Tracking gain (anti-windup)
+    float kFF;  // FeedForward Component
 } pidControllerParam_t;
 
 typedef struct {
@@ -256,6 +257,7 @@ typedef struct {
     float integral;                     // used integral value in output
     float proportional;                 // used proportional value in output
     float derivative;                   // used derivative value in output
+    float feedForward;                  // used FeedForward value in output
     float output_constrained;           // controller output constrained
 } pidController_t;
 

src/main/navigation/navigation_private.h

@@ -353,7 +353,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
 float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
 float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler);
 void navPidReset(pidController_t *pid);
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD);
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF);
 void navPInit(pController_t *p, float _kP);
 
 bool isThrustFacingDownwards(void);