Make yaw controller configurable

src/main/build/debug.h

@@ -67,5 +67,6 @@ typedef enum {
     DEBUG_ERPM,
     DEBUG_RPM_FILTER,
     DEBUG_RPM_FREQ,
+    DEBUG_NAV_YAW,
     DEBUG_COUNT
 } debugType_e;

src/main/fc/settings.yaml

@@ -81,7 +81,7 @@ tables:
     values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
-      "ERPM", "RPM_FILTER", "RPM_FREQ"]
+      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -1244,6 +1244,10 @@ groups:
         condition: USE_NAV
         min: 0
         max: 255
+      - name: nav_fw_pos_hdg_pidsum_limit
+        field: pidSumLimitYaw
+        min: PID_SUM_LIMIT_MIN
+        max: PID_SUM_LIMIT_MAX
       - name: mc_iterm_relax_type
         field: iterm_relax_type
         table: iterm_relax_type
@@ -1660,6 +1664,9 @@ groups:
       - name: nav_fw_allow_manual_thr_increase
         field: fw.allow_manual_thr_increase
         type: bool
+      - name: nav_use_fw_yaw_control
+        field: fw.useFwNavYawControl
+        type: bool
 
   - name: PG_TELEMETRY_CONFIG
     type: telemetryConfig_t

src/main/flight/pid.c

@@ -146,7 +146,7 @@ typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t
 static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 12);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -263,6 +263,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .antigravityAccelerator = 1.0f,
         .antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
         .pidControllerType = PID_TYPE_AUTO,
+        .navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT,
 );
 
 bool pidInitFilters(void)

src/main/flight/pid.h

@@ -143,6 +143,8 @@ typedef struct pidProfile_s {
     float antigravityGain;
     float antigravityAccelerator;
     uint8_t antigravityCutoff;
+
+    int navFwPosHdgPidsumLimit;
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {

src/main/navigation/navigation.c

@@ -80,7 +80,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS];
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -165,7 +165,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .launch_climb_angle = 18,               // 18 degrees
         .launch_max_angle = 45,                 // 45 deg
         .cruise_yaw_rate  = 20,                 // 20dps
-        .allow_manual_thr_increase = false
+        .allow_manual_thr_increase = false,
+        .useFwNavYawControl = 0,
     }
 );
 
@@ -1775,6 +1776,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
         }
     }
 
+    /*
+     * Limit both output and Iterm to limit windup
+     */
+    pid->integrator = constrain(pid->integrator, outMin, outMax);
+
     return outValConstrained;
 }
 
@@ -3211,7 +3217,7 @@ void navigationUsePIDs(void)
                                         (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
+                                        2.0f
     );
 }
 

src/main/navigation/navigation.h

@@ -216,6 +216,7 @@ typedef struct navConfig_s {
         uint8_t  launch_max_angle;           // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
         uint8_t  cruise_yaw_rate;            // Max yaw rate (dps) when CRUISE MODE is enabled
         bool     allow_manual_thr_increase;
+        bool useFwNavYawControl;
     } fw;
 } navConfig_t;
 

src/main/navigation/navigation_fixedwing.c

@@ -210,8 +210,10 @@ void resetFixedWingPositionController(void)
     virtualDesiredPosition.z = 0;
 
     navPidReset(&posControl.pids.fw_nav);
+    navPidReset(&posControl.pids.fw_heading);
     posControl.rcAdjustment[ROLL] = 0;
     posControl.rcAdjustment[YAW] = 0;
+    posControl.rcAdjustment[YAW] = 0;
     isRollAdjustmentValid = false;
     isYawAdjustmentValid = false;
 
@@ -295,7 +297,10 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
     // We have virtual position target, calculate heading error
     int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
 
-    // Calculate NAV heading error
+    /*
+     * Calculate NAV heading error
+     * Units are centidegrees
+     */
     navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
 
     // Forced turn direction
@@ -341,12 +346,34 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
      * 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),
+    if (navConfig()->fw.useFwNavYawControl) {
-                                       -500,
+
-                                        500,
+        static float limit = 0.0f;
-                                        pidFlags);
+
+        if (limit == 0.0f) {
+            limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
+        }
+
+        float yawAdjustment = navPidApply2(
+            &posControl.pids.fw_heading, 
+            0, 
+            navHeadingError, 
+            US2S(deltaMicros),
+            -limit,
+            limit,
+            pidFlags
+            ) / 100.0f;
+
+        DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
+        DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
+        DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
+        DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
+        DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
 
         posControl.rcAdjustment[YAW] = yawAdjustment;
+    } else {
+        posControl.rcAdjustment[YAW] = 0;
+    }
 }
 
 void applyFixedWingPositionController(timeUs_t currentTimeUs)