FW nav: replace roll PT1 smoothing with a triggered S-curve (no steady-state lag)

docs/Settings.md

@@ -3571,6 +3571,16 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci
 
 ---
 
+### nav_cruise_lock_on_level
+
+Fixed wing only: when ON the COURSE HOLD/CRUISE course is locked only once the aircraft has rolled out level (below 10 deg bank) after a heading adjustment or a banked mode entry, following the actual course until then. Prevents overshooting the locked course during the level-off. OFF locks the course as soon as the sticks are centered (legacy behaviour).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| ON | OFF | ON |
+
+---
+
 ### nav_cruise_yaw_rate
 
 Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]
@@ -3704,7 +3714,7 @@ Max pitch angle when climbing in GPS assisted modes, is also restrained by globa
 
 ### nav_fw_control_smoothness
 
-How smoothly the autopilot controls the airplane to correct the navigation error
+How smoothly the autopilot corrects the navigation error. Pitch uses a low-pass filter. Roll uses an S-curve easing window of n x 100 ms (max 900 ms) applied only when the commanded bank changes abruptly, so steady course tracking is never lagged. 0 = no roll smoothing.
 
 | Default | Min | Max |
 | --- | --- | --- |

src/main/fc/settings.yaml

@@ -2958,6 +2958,11 @@ groups:
         field: general.cruise_yaw_rate
         min: 0
         max: 120
+      - name: nav_cruise_lock_on_level
+        description: "Fixed wing only: when ON the COURSE HOLD/CRUISE course is locked only once the aircraft has rolled out level (below 10 deg bank) after a heading adjustment or a banked mode entry, following the actual course until then. Prevents overshooting the locked course during the level-off. OFF locks the course as soon as the sticks are centered (legacy behaviour)."
+        default_value: ON
+        field: general.cruise_lock_on_level
+        type: bool
       - name: nav_mc_bank_angle
         description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
         default_value: 35
@@ -3145,7 +3150,7 @@ groups:
         min: 1
         max: 10
       - name: nav_fw_control_smoothness
-        description: "How smoothly the autopilot controls the airplane to correct the navigation error"
+        description: "How smoothly the autopilot corrects the navigation error. Pitch uses a low-pass filter. Roll uses an S-curve easing window of n x 100 ms (max 900 ms) applied only when the commanded bank changes abruptly, so steady course tracking is never lagged. 0 = no roll smoothing."
         default_value: 0
         field: fw.control_smoothness
         min: 0

src/main/navigation/navigation.c

@@ -92,6 +92,9 @@
 #define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec)
 #define FW_LAND_LOITER_ALT_TOLERANCE 150
 
+#define FW_COURSE_LOCK_MAX_BANK_DECIDEG 100 // lock the cruise course only once rolled out below this bank angle (10 deg)
+#define FW_COURSE_LOCK_FORCE_TIMEOUT_MS 2500 // failsafe: force the lock this long after stick centre even if still banked
+
 #ifdef USE_AUTO_TRANSITION
 // One-shot MC->FW mission retry after airspeed-timeout: yaw scan, align to best pitot heading.
 #define NAV_MIXERAT_RETRY_SCAN_STEP_CD       DEGREES_TO_CENTIDEGREES(20)
@@ -138,10 +141,12 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
 #endif
 
+// One above upstream's respective version in both variants: this branch adds unconditional
+// navConfig fields on top of either struct layout
 #ifdef USE_AUTO_TRANSITION
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 11);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 12);
 #else
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9);
 #endif
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
@@ -207,6 +212,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
         .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT,                                 // 20dps
         .rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT,                       // Delay before landing in FS. 0 = immedate landing
+        .cruise_lock_on_level = SETTING_NAV_CRUISE_LOCK_ON_LEVEL_DEFAULT,
     },
 
     // MC-specific
@@ -1552,6 +1558,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
     return NAV_FSM_EVENT_NONE;
 }
 
+// FW course hold: the course lock is pending while a turn is still being rolled out (mode entry from
+// a banked turn or heading adjustment just released) - the course follows the actual COG until then.
+// Gated by nav_cruise_lock_on_level; when OFF the course locks as soon as the sticks are centered.
+static bool fwCruiseCourseLockPending = false;
+static bool fwCruiseHeadingAdjustActive = false; // yaw adjustment running: its sustained course lead winds up the nav PID
+static timeMs_t fwCruiseStickCentreTimeMs = 0;  // last time the roll/yaw sticks went to centre
+
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
@@ -1570,6 +1583,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
 
     if (STATE(AIRPLANE)) {
         posControl.cruise.course = posControl.actualState.cog;  // Store the course to follow
+        // Entering from a banked turn (e.g. mode switch out of RTH mid-turn): course hold means
+        // "fly straight from here", so follow the COG until the roll-out is complete, then lock.
+        fwCruiseCourseLockPending = navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG;
+        fwCruiseHeadingAdjustActive = false;
+        fwCruiseStickCentreTimeMs = millis();
     } else {    // Multicopter
         posControl.cruise.course = posControl.actualState.yaw;
         posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed);
@@ -1600,7 +1618,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
     }
 
     const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband;
-    static bool adjustmentWasActive = false;
 
     // User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR
     // We record the desired course and change the desired target in the meanwhile
@@ -1621,13 +1638,25 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
         }
 
         posControl.cruise.lastCourseAdjustmentTime = currentTimeMs;
-        adjustmentWasActive = true;
+        fwCruiseCourseLockPending = true;
+        fwCruiseHeadingAdjustActive = true;
+        fwCruiseStickCentreTimeMs = currentTimeMs;
 
         DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
-    } else if (STATE(AIRPLANE) && adjustmentWasActive) {
-        posControl.cruise.course = posControl.actualState.cog - DEGREES_TO_CENTIDEGREES(gyroRateDps(YAW));
-        resetPositionController();
-        adjustmentWasActive = false;
+    } else if (STATE(AIRPLANE) && fwCruiseCourseLockPending) {
+        // Locking while still banked overshoots the course; the timeout covers a roll-out that never completes
+        const bool rolledOut = !navConfig()->general.cruise_lock_on_level
+                               || ABS(attitude.values.roll) <= FW_COURSE_LOCK_MAX_BANK_DECIDEG
+                               || currentTimeMs - fwCruiseStickCentreTimeMs >= FW_COURSE_LOCK_FORCE_TIMEOUT_MS;
+        // Without the level gate the turn continues through the roll-out: keep the legacy yaw-rate lead
+        posControl.cruise.course = navConfig()->general.cruise_lock_on_level ? posControl.actualState.cog
+                                   : posControl.actualState.cog - DEGREES_TO_CENTIDEGREES(gyroRateDps(YAW));
+
+        if (fwCruiseHeadingAdjustActive || rolledOut) {
+            resetPositionController();      // the adjustment wound up the integrator; a zero error cannot unwind it
+            fwCruiseHeadingAdjustActive = false;
+        }
+        fwCruiseCourseLockPending = !rolledOut;
     } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) {
         posControl.cruise.previousCourse = posControl.cruise.course;
     }
@@ -1642,10 +1671,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
     UNUSED(previousState);
     DEBUG_SET(DEBUG_CRUISE, 0, 3);
 
-    // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
+    // User is rolling, changing manually direction. Wait until it is done AND the roll-out is
+    // complete before locking the course and re-engaging: a course locked while still banked is
+    // overshot during the level-off (the turn continues), forcing a reverse correction.
     if (posControl.flags.isAdjustingPosition) {
         posControl.cruise.course = posControl.actualState.cog;  //store current course
         posControl.cruise.lastCourseAdjustmentTime = millis();
+        fwCruiseStickCentreTimeMs = millis();
+        return NAV_FSM_EVENT_NONE;  // reprocess the state
+    }
+    if (STATE(AIRPLANE) && navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG
+        && millis() - fwCruiseStickCentreTimeMs < FW_COURSE_LOCK_FORCE_TIMEOUT_MS) {
+        posControl.cruise.course = posControl.actualState.cog;
+        posControl.cruise.lastCourseAdjustmentTime = millis();
         return NAV_FSM_EVENT_NONE;  // reprocess the state
     }
 

src/main/navigation/navigation.h

@@ -473,6 +473,7 @@ typedef struct navConfig_s {
         uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately)
         uint8_t  cruise_yaw_rate;                   // Max yaw rate (dps) when CRUISE MODE is enabled
         uint16_t rth_fs_landing_delay;              // Delay upon reaching home before starting landing if in FS (0 = immediate)
+        bool     cruise_lock_on_level;              // FW: lock the course hold course only once rolled out level (OFF = lock on stick release)
     } general;
 
     struct {

src/main/navigation/navigation_fixedwing.c

@@ -57,9 +57,16 @@
 
 #include "sensors/battery.h"
 
-// Base frequencies for smoothing pitch and roll
+// Base frequency for smoothing the pitch command and the pitch-to-throttle correction
 #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ     2.0f
-#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ     10.0f
+
+// Roll-command S-curve smoothing: control_smoothness (0..9) -> easing window = n*100 ms (0 = off,
+// max 900 ms). Triggered only on an abrupt commanded-bank step (>20% of the configured roll
+// rate between nav loops), then eased over the window and passed 1:1 afterwards. Unlike the previous
+// PT1 low-pass this never lags steady tracking, so the controller command stays deterministic.
+#define NAV_FW_SMOOTH_TCONST_PER_STEP_MS  100.0f
+#define NAV_FW_SMOOTH_TCONST_MAX_MS       900.0f
+#define NAV_FW_SMOOTH_STEP_FRACTION       0.2f
 
 // If we are going slower than the minimum ground speed (navConfig()->general.min_ground_speed) - boost throttle to fight against the wind
 #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN        1.5f
@@ -73,6 +80,10 @@ static bool isYawAdjustmentValid = false;
 static float throttleSpeedAdjustment = 0;
 static bool isAutoThrottleManuallyIncreased = false;
 static float navCrossTrackError;
+static bool fwRollSmoothReseed = false;     // re-sync the roll S-curve smoother on the next frame (after a controller reset)
+static float fwRollSmoothSeedCd = 0.0f;     // baseline the smoother re-seeds to (set by the controller reset)
+static float fwLastNavRollCmdCd = 0.0f;     // last applied nav roll command [centideg] + timestamp, to tell a
+static timeUs_t fwLastNavRollCmdTimeUs = 0; // nav-to-nav transition apart from a pilot handover at reset time
 static int8_t loiterDirYaw = 1;
 static bool needToCalculateCircularLoiter;
 static bool autoSpeedIsActive = false;
@@ -267,7 +278,6 @@ bool adjustFixedWingHeadingFromRCInput(void)
  * XY-position controller
  *-----------------------------------------------------------*/
 static fpVector3_t virtualDesiredPosition;
-static pt1Filter_t fwPosControllerCorrectionFilterState;
 static pt1Filter_t fwCrossTrackErrorRateFilterState;
 
 /*
@@ -286,10 +296,14 @@ void resetFixedWingPositionController(void)
     isRollAdjustmentValid = false;
     isYawAdjustmentValid = false;
 
+    // Re-seed the roll S-curve smoother. If nav commanded roll until just now (nav-mode to nav-mode
+    // transition, e.g. RTH -> CRUISE) seed from the last applied command so the level-off/turn change
+    // is eased; after a pilot-flown phase seed neutral so a roll-out in progress is not re-commanded.
+    fwRollSmoothSeedCd = ((micros() - fwLastNavRollCmdTimeUs) < MAX_POSITION_UPDATE_INTERVAL_US) ? fwLastNavRollCmdCd : 0.0f;
+    fwRollSmoothReseed = true;
+
     pt1FilterSetCutoff(&fwCrossTrackErrorRateFilterState, 3.0f);
     pt1FilterReset(&fwCrossTrackErrorRateFilterState, 0.0f);
-    pt1FilterSetCutoff(&fwPosControllerCorrectionFilterState, getSmoothnessCutoffFreq(NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ));
-    pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
 }
 
 static int8_t loiterDirection(void) {
@@ -325,6 +339,62 @@ static int8_t loiterDirection(void) {
     return dir;
 }
 
+// Triggered S-curve roll-in [centideg]: on an abrupt commanded-bank step (new heading), ease toward the
+// target with a smoothstep over a control_smoothness-derived time constant, then pass 1:1. The timer is
+// not reset by further steps mid-ramp, so we never get stuck damping steady tracking.
+static float applyFwRollInSmoothing(float rollTargetCd, timeDelta_t deltaMicros, bool reseed)
+{
+    static float prevTarget = 0.0f;
+    static float prevRate = 0.0f;
+    static float rampStart = 0.0f;
+    static float prevOut = 0.0f;
+    static float elapsedMs = 0.0f;
+    static bool active = false;
+
+    if (reseed) {                                               // controller reset: re-seed to the baseline chosen at reset time
+        active = false;                                          // (last nav command on a nav-to-nav transition, else neutral), so
+        elapsedMs = 0.0f;                                        // the cross-mode command step is detected and eased while stale
+        prevTarget = fwRollSmoothSeedCd;                         // state can never fire a spurious ramp
+        prevRate = 0.0f;
+        prevOut = fwRollSmoothSeedCd;
+    }
+
+    const float tConstMs = MIN((float)navConfig()->fw.control_smoothness * NAV_FW_SMOOTH_TCONST_PER_STEP_MS, NAV_FW_SMOOTH_TCONST_MAX_MS);
+    const float dtS = US2S(deltaMicros);
+    if (tConstMs <= 0.0f || dtS <= 0.0f) {                       // smoothing off: pass through
+        active = false;
+        prevTarget = rollTargetCd;
+        prevRate = 0.0f;
+        prevOut = rollTargetCd;
+        return rollTargetCd;
+    }
+
+    const float cmdRate = (rollTargetCd - prevTarget) / dtS;     // commanded bank rate [centideg/s]
+    const float stepThreshold = NAV_FW_SMOOTH_STEP_FRACTION * (currentControlProfile->stabilized.rates[FD_ROLL] * 10.0f) * 100.0f;  // 20% of roll rate [centideg/s]
+    if (!active && fabsf(cmdRate - prevRate) > stepThreshold) {  // abrupt setpoint-rate change -> start the S-curve
+        active = true;
+        elapsedMs = 0.0f;
+        rampStart = prevOut;
+    }
+
+    float out = rollTargetCd;                                    // default: 1:1 pass-through
+    if (active) {
+        elapsedMs += dtS * 1000.0f;                             // timer does NOT reset on further steps
+        if (elapsedMs >= tConstMs) {
+            active = false;                                      // window elapsed -> back to 1:1
+        } else {
+            const float p = elapsedMs / tConstMs;
+            const float s = p * p * (3.0f - 2.0f * p);          // smoothstep (S-curve)
+            out = rampStart + s * (rollTargetCd - rampStart);
+        }
+    }
+
+    prevTarget = rollTargetCd;
+    prevRate = cmdRate;
+    prevOut = out;
+    return out;
+}
+
 static void calculateVirtualPositionTarget_FW(float trackingPeriod)
 {
     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) {
@@ -566,11 +636,15 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
                                         DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
                                         pidFlags);
 
-    // Apply low-pass filter to prevent rapid correction
-    rollAdjustment = pt1FilterApply3(&fwPosControllerCorrectionFilterState, rollAdjustment, US2S(deltaMicros));
+    // Triggered S-curve smoothing on the roll command (control_smoothness); re-seeded after a
+    // controller reset so stale smoother state cannot fire a spurious ramp.
+    rollAdjustment = applyFwRollInSmoothing(rollAdjustment, deltaMicros, fwRollSmoothReseed);
+    fwRollSmoothReseed = false;
 
     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
+    fwLastNavRollCmdCd = rollAdjustment;
+    fwLastNavRollCmdTimeUs = currentTimeUs;
 
     /*
      * Yaw adjustment