Initial cut for NAV yaw adjustments with separate PID controller on FW

src/main/fc/settings.yaml

@@ -1228,6 +1228,21 @@ groups:
         condition: USE_NAV
         min: 0
         max: 255
+      - name: nav_fw_pos_hdg_p
+        field: bank_fw.pid[PID_POS_HEADING].P
+        condition: USE_NAV
+        min: 0
+        max: 255
+      - name: nav_fw_pos_hdg_i
+        field: bank_fw.pid[PID_POS_HEADING].I
+        condition: USE_NAV
+        min: 0
+        max: 255
+      - name: nav_fw_pos_hdg_d
+        field: bank_fw.pid[PID_POS_HEADING].D
+        condition: USE_NAV
+        min: 0
+        max: 255
       - name: mc_iterm_relax_type
         field: iterm_relax_type
         table: iterm_relax_type

src/main/flight/pid.c

@@ -184,6 +184,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                     .I = 50,    // NAV_VEL_Z_I * 20
                     .D = 10,    // NAV_VEL_Z_D * 100
                     .FF = 0,
+                },
+                [PID_POS_HEADING] = {
+                    .P = 0,
+                    .I = 0,
+                    .D = 0,
+                    .FF = 0
                 }
             }
         },
@@ -211,6 +217,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                     .I = 5,     // FW_POS_XY_I * 100
                     .D = 8,     // FW_POS_XY_D * 100
                     .FF = 0,
+                },
+                [PID_POS_HEADING] = {
+                    .P = 20,
+                    .I = 10,
+                    .D = 10,
+                    .FF = 0
                 }
             }
         },

src/main/flight/pid.h

@@ -67,6 +67,7 @@ typedef enum {
     PID_LEVEL,      //   +       +
     PID_HEADING,    //   +       +
     PID_VEL_Z,      //   +       n/a
+    PID_POS_HEADING,//   n/a     +
     PID_ITEM_COUNT
 } pidIndex_e;
 

src/main/navigation/navigation.c

@@ -3195,6 +3195,13 @@ void navigationUsePIDs(void)
                                         0.0f,
                                         NAV_DTERM_CUT_HZ
     );
+
+    navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 100.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 100.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
+                                        0.0f,
+                                        NAV_DTERM_CUT_HZ
+    );
 }
 
 void navigationInit(void)

src/main/navigation/navigation.h

@@ -306,6 +306,7 @@ typedef struct navigationPIDControllers_s {
     /* Fixed-wing PIDs */
     pidController_t fw_alt;
     pidController_t fw_nav;
+    pidController_t fw_heading;
 } navigationPIDControllers_t;
 
 /* MultiWii-compatible params for telemetry */

src/main/navigation/navigation_fixedwing.c

@@ -61,6 +61,7 @@
 
 static bool isPitchAdjustmentValid = false;
 static bool isRollAdjustmentValid = false;
+static bool isYawAdjustmentValid = false;
 static float throttleSpeedAdjustment = 0;
 static bool isAutoThrottleManuallyIncreased = false;
 static int32_t navHeadingError;
@@ -210,7 +211,9 @@ void resetFixedWingPositionController(void)
 
     navPidReset(&posControl.pids.fw_nav);
     posControl.rcAdjustment[ROLL] = 0;
+    posControl.rcAdjustment[YAW] = 0;
     isRollAdjustmentValid = false;
+    isYawAdjustmentValid = false;
 
     pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
 }
@@ -333,6 +336,17 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
+
+    /*
+     * Yaw adjustment
+     * It is working in relative mode and we aim to keep error at zero
+     */
+    float yawAdjustment = navPidApply2(&posControl.pids.fw_heading, 0, navHeadingError, US2S(deltaMicros),
+                                       -500,
+                                        500,
+                                        pidFlags);
+
+    posControl.rcAdjustment[YAW] = yawAdjustment;
 }
 
 void applyFixedWingPositionController(timeUs_t currentTimeUs)
@@ -376,10 +390,12 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
         }
 
         isRollAdjustmentValid = true;
+        isYawAdjustmentValid = true;
     }
     else {
         // No valid pos sensor data, don't adjust pitch automatically, rcCommand[ROLL] is passed through to PID controller
         isRollAdjustmentValid = false;
+        isYawAdjustmentValid = false;
     }
 }
 
@@ -448,6 +464,10 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
         rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
     }
 
+    if (isYawAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
+        rcCommand[YAW] = posControl.rcAdjustment[YAW];
+    }
+
     if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
         // PITCH >0 dive, <0 climb
         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));