From 10a065fbe8d6f42c6ec4223755675ea1d3143b5d Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 20 Aug 2026 16:24:43 +0200 Subject: [PATCH 1/7] FW nav: replace roll PT1 smoothing with a triggered S-curve (no steady-state lag) nav_fw_control_smoothness applied a PT1 low-pass to the FW nav roll command. That trades smoothness for a permanent, uncompensated lag between what the navigation controller commands and what is executed: every course correction is delayed, also during steady tracking where no smoothing is needed, and the lag grows with the smoothness setting. Replace the roll-axis PT1 with a triggered S-curve easing: - Fires only on an abrupt commanded-bank step (setpoint-rate change above 20% of the configured roll rate between nav loops), e.g. a new course at a waypoint or a nav-mode entry (RTH engage, WP start). - Eases from the pre-step output to the live target with a smoothstep over a control_smoothness-derived window (n x 100 ms, 0 = off, capped at 1000 ms), then passes the command 1:1 again. - The window timer does not reset on further steps mid-ramp, so the smoother can never get stuck damping steady tracking. - On position-controller reset the smoother re-seeds from the last applied nav roll command when nav was commanding until just now (nav-mode to nav-mode transition, e.g. RTH -> CRUISE: the level-off is eased), and from the neutral baseline after a pilot-flown phase (stick release: a roll-out in progress is not re-commanded). Stale state can never fire a spurious ramp. Same knob, same range and same intent (soft control feel, structural protection on large airframes); the pitch/pitch-to-throttle PT1 smoothing is deliberately unchanged. No settings or PG layout changes. HITL-tested on real hardware (window rescaled to n x 100 ms from flight observation; re-seed behavior derived from RTH engage, cruise stick release and RTH->CRUISE fallback tests). --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_fixedwing.c | 88 ++++++++++++++++++++-- 3 files changed, 83 insertions(+), 9 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 2df6b7ebf4f..a4d82da3b3c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3640,7 +3640,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 | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 8849ccdaf28..a617e3be510 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3056,7 +3056,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 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0451f8a94ab..ae91941456c 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -56,9 +56,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), +// capped at 1000 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 1000.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 @@ -72,6 +79,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; @@ -261,7 +272,6 @@ bool adjustFixedWingHeadingFromRCInput(void) * XY-position controller *-----------------------------------------------------------*/ static fpVector3_t virtualDesiredPosition; -static pt1Filter_t fwPosControllerCorrectionFilterState; static pt1Filter_t fwCrossTrackErrorRateFilterState; /* @@ -280,10 +290,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) { @@ -319,6 +333,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) { @@ -560,11 +630,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 From 4c9288af8d53f9f6af8871f130a18c176e5d3714 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 20 Aug 2026 19:18:12 +0200 Subject: [PATCH 2/7] FW nav: lock cruise course only once rolled out (below 10 deg bank) In COURSE_HOLD/CRUISE the course is locked the moment the mode engages or the pilot releases the stick (roll-stick path: last course stored in ADJUSTING; yaw path: on release with a one-iteration gyro lead; mode entry: in INITIALIZE). If the aircraft is still banked at that moment - stick released mid-turn, or the mode switched out of e.g. an RTH turn - it keeps turning through the level-off, overshoots the locked course and flies a reverse correction turn. A longstanding annoyance, made more visible by softer roll-out (control smoothing). Delay the course lock until the roll-out is actually complete: while the bank is above 10 deg the course keeps following the actual COG (roll-stick path stays in ADJUSTING; yaw release and banked mode entry share one lock-pending flag), then locks with the gyro-lead compensation. The course now locks where the aircraft has effectively stopped turning - no overshoot, no reverse correction - and the controller reset/re-engage happens near wings-level, so the smoothing re-seed cannot cause a roll jerk. Fixed-wing only; multicopter course hold is unaffected. --- src/main/navigation/navigation.c | 32 ++++++++++++++++++++++++-------- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e112ce8a1b9..ae3e98e0cdc 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -83,6 +83,8 @@ #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) + /*----------------------------------------------------------- * Compatibility for home position *-----------------------------------------------------------*/ @@ -1371,6 +1373,10 @@ 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. +static bool fwCruiseCourseLockPending = false; + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState) { UNUSED(previousState); @@ -1389,6 +1395,9 @@ 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 = ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG; } else { // Multicopter posControl.cruise.course = posControl.actualState.yaw; posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed); @@ -1419,7 +1428,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 @@ -1440,13 +1448,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS } posControl.cruise.lastCourseAdjustmentTime = currentTimeMs; - adjustmentWasActive = true; + fwCruiseCourseLockPending = true; 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) { + if (ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG) { + // Still banked (adjustment turn or banked mode entry): keep following the actual course + // until the roll-out is complete, else the locked course is overshot and reverse-corrected. + posControl.cruise.course = posControl.actualState.cog; + } else { + posControl.cruise.course = posControl.actualState.cog - DEGREES_TO_CENTIDEGREES(gyroRateDps(YAW)); + resetPositionController(); + fwCruiseCourseLockPending = false; + } } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) { posControl.cruise.previousCourse = posControl.cruise.course; } @@ -1461,8 +1475,10 @@ 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 - if (posControl.flags.isAdjustingPosition) { + // 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 || (STATE(AIRPLANE) && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG)) { posControl.cruise.course = posControl.actualState.cog; //store current course posControl.cruise.lastCourseAdjustmentTime = millis(); return NAV_FSM_EVENT_NONE; // reprocess the state From 37df83709877b2f9fe137513d8093b719d3a4713 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 20 Aug 2026 20:35:51 +0200 Subject: [PATCH 3/7] FW nav: address review - drop yaw-rate lead from course lock, align easing cap to 900ms The course-lock applied 'cog - DEGREES_TO_CENTIDEGREES(gyroRateDps(YAW))', mixing a rate (deg/s) into an angle - effectively a fixed one-second yaw lead. With the new bank gate the turn has essentially stopped at lock time, so lock directly to the current COG. NAV_FW_SMOOTH_TCONST_MAX_MS claimed a 1000ms cap that was unreachable with control_smoothness max 9 (n x 100ms = 900ms); set the cap and comments to 900ms to match the setting range and documentation. --- src/main/navigation/navigation.c | 4 +++- src/main/navigation/navigation_fixedwing.c | 6 +++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ae3e98e0cdc..731481a9219 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1457,7 +1457,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS // until the roll-out is complete, else the locked course is overshot and reverse-corrected. posControl.cruise.course = posControl.actualState.cog; } else { - posControl.cruise.course = posControl.actualState.cog - DEGREES_TO_CENTIDEGREES(gyroRateDps(YAW)); + // Rolled out: lock to the current COG. The former yaw-rate lead term mixed a rate into an + // angle; with the bank gate the residual turn rate at lock time is negligible anyway. + posControl.cruise.course = posControl.actualState.cog; resetPositionController(); fwCruiseCourseLockPending = false; } diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ae91941456c..c58bd00fc7b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -59,12 +59,12 @@ // Base frequency for smoothing the pitch command and the pitch-to-throttle correction #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f -// Roll-command S-curve smoothing: control_smoothness (0..9) -> easing window = n*100 ms (0 = off), -// capped at 1000 ms. Triggered only on an abrupt commanded-bank step (>20% of the configured roll +// 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 1000.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 From fb3ccf062cb5ba9dbae298544a3e67d2d56555af Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Fri, 21 Aug 2026 13:29:50 +0200 Subject: [PATCH 4/7] FW nav: gate course-lock-on-level behind nav_cruise_lock_on_level (default ON) On maintainer feedback the level-off course lock in course hold is a behavior change, so make it optional: ON locks the course only once rolled out below 10 deg bank (new behavior), OFF locks on stick center / mode entry as before. Bumps PG_NAV_CONFIG to 9. Co-Authored-By: Claude Fable 5 --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 5 +++++ src/main/navigation/navigation.c | 10 ++++++---- src/main/navigation/navigation.h | 1 + 4 files changed, 22 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index a4d82da3b3c..eaf52e6e8b6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3507,6 +3507,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] diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a617e3be510..5f6eae0cbbc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2869,6 +2869,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 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 731481a9219..a08d483d995 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -121,7 +121,7 @@ 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 -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -179,6 +179,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 @@ -1375,6 +1376,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( // 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 navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState) @@ -1397,7 +1399,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( 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 = ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG; + fwCruiseCourseLockPending = navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG; } else { // Multicopter posControl.cruise.course = posControl.actualState.yaw; posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed); @@ -1452,7 +1454,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); } else if (STATE(AIRPLANE) && fwCruiseCourseLockPending) { - if (ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG) { + if (navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG) { // Still banked (adjustment turn or banked mode entry): keep following the actual course // until the roll-out is complete, else the locked course is overshot and reverse-corrected. posControl.cruise.course = posControl.actualState.cog; @@ -1480,7 +1482,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n // 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 || (STATE(AIRPLANE) && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG)) { + if (posControl.flags.isAdjustingPosition || (STATE(AIRPLANE) && navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG)) { posControl.cruise.course = posControl.actualState.cog; //store current course posControl.cruise.lastCourseAdjustmentTime = millis(); return NAV_FSM_EVENT_NONE; // reprocess the state diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b6cf4692b66..55c34dc6d64 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -441,6 +441,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 { From 481e544240bbfffa728bf03368059c7a79a6ee34 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 24 Aug 2026 10:34:56 +0200 Subject: [PATCH 5/7] FW nav: course lock failsafe - force the lock 2.5s after stick centre A badly tuned model in strong wind can stay above the 10 deg bank gate indefinitely, leaving the course lock pending. Once the roll/yaw sticks are centred the lock now engages after 2500 ms regardless of bank. Co-Authored-By: Claude Fable 5 --- src/main/navigation/navigation.c | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a08d483d995..bb2e1ce447b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -84,6 +84,7 @@ #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 /*----------------------------------------------------------- * Compatibility for home position @@ -1378,6 +1379,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( // 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 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) { @@ -1400,6 +1402,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( // 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; + fwCruiseStickCentreTimeMs = millis(); } else { // Multicopter posControl.cruise.course = posControl.actualState.yaw; posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed); @@ -1451,12 +1454,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS posControl.cruise.lastCourseAdjustmentTime = currentTimeMs; fwCruiseCourseLockPending = true; + fwCruiseStickCentreTimeMs = currentTimeMs; DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); } else if (STATE(AIRPLANE) && fwCruiseCourseLockPending) { - if (navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG) { + if (navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG + && currentTimeMs - fwCruiseStickCentreTimeMs < FW_COURSE_LOCK_FORCE_TIMEOUT_MS) { // Still banked (adjustment turn or banked mode entry): keep following the actual course // until the roll-out is complete, else the locked course is overshot and reverse-corrected. + // The timeout forces the lock if the roll-out never completes (badly tuned model in wind). posControl.cruise.course = posControl.actualState.cog; } else { // Rolled out: lock to the current COG. The former yaw-rate lead term mixed a rate into an @@ -1482,9 +1488,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n // 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 || (STATE(AIRPLANE) && navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG)) { + 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 } From ead7f89454383cec41f8801d8ca2033c9c5735d3 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 24 Aug 2026 11:24:46 +0200 Subject: [PATCH 6/7] FW nav: clear the nav PID integrator when a course adjustment ends A yaw-stick turn holds the course setpoint ahead of the actual COG, winding up the nav PID integrator to the bank that sustains the turn. Once the stick centres the course follows COG, so the error is zero and neither the error term nor back-calculation (only active while saturated) can unwind it: the residual bank kept the turn going and blocked the roll-out the course lock waits for. Reset the position controller once when the adjustment ends, as the pre-existing code did before the lock-on-level gate was introduced. Co-Authored-By: Claude Fable 5 --- src/main/navigation/navigation.c | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bb2e1ce447b..d17db63314f 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1379,6 +1379,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( // 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) @@ -1402,6 +1403,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( // 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; @@ -1454,23 +1456,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS posControl.cruise.lastCourseAdjustmentTime = currentTimeMs; fwCruiseCourseLockPending = true; + fwCruiseHeadingAdjustActive = true; fwCruiseStickCentreTimeMs = currentTimeMs; DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); } else if (STATE(AIRPLANE) && fwCruiseCourseLockPending) { - if (navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG - && currentTimeMs - fwCruiseStickCentreTimeMs < FW_COURSE_LOCK_FORCE_TIMEOUT_MS) { - // Still banked (adjustment turn or banked mode entry): keep following the actual course - // until the roll-out is complete, else the locked course is overshot and reverse-corrected. - // The timeout forces the lock if the roll-out never completes (badly tuned model in wind). - posControl.cruise.course = posControl.actualState.cog; - } else { - // Rolled out: lock to the current COG. The former yaw-rate lead term mixed a rate into an - // angle; with the bank gate the residual turn rate at lock time is negligible anyway. - posControl.cruise.course = posControl.actualState.cog; - resetPositionController(); - fwCruiseCourseLockPending = false; + // Follow the actual course until rolled out, else the locked course is overshot and + // reverse-corrected; the timeout forces the lock if the roll-out 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; + posControl.cruise.course = posControl.actualState.cog; + + if (fwCruiseHeadingAdjustActive || rolledOut) { + resetPositionController(); // the adjustment's course lead wound up the integrator; a zero error can never unwind it + fwCruiseHeadingAdjustActive = false; } + fwCruiseCourseLockPending = !rolledOut; } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) { posControl.cruise.previousCourse = posControl.cruise.course; } From 175feed87b7a82cbb2b7d536c67dbb9173aae4cd Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Mon, 24 Aug 2026 13:50:14 +0200 Subject: [PATCH 7/7] FW nav: keep the legacy yaw-rate lead when the level gate is off nav_cruise_lock_on_level makes the roll-out gate optional, so with it off the course locks immediately while the aircraft is still turning - the case the 'cog - gyroRateDps(YAW)' lead was there for. Restore it for that path; the gated path locks after the roll-out and the forced timeout lock does not need it either (2.5 s of level command means level or already in trouble). Co-Authored-By: Claude Fable 5 --- src/main/navigation/navigation.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d17db63314f..51cf54a8e73 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1461,15 +1461,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); } else if (STATE(AIRPLANE) && fwCruiseCourseLockPending) { - // Follow the actual course until rolled out, else the locked course is overshot and - // reverse-corrected; the timeout forces the lock if the roll-out never completes. + // 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; - posControl.cruise.course = posControl.actualState.cog; + // 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's course lead wound up the integrator; a zero error can never unwind it + resetPositionController(); // the adjustment wound up the integrator; a zero error cannot unwind it fwCruiseHeadingAdjustActive = false; } fwCruiseCourseLockPending = !rolledOut;