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 01/21] 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 02/21] 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 03/21] 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 04/21] 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 93e9bca69813674367cea51b554ca47b3f72d87d Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 9 Jun 2026 16:14:28 +0200 Subject: [PATCH 05/21] FW nav: speed-aware waypoint turn predictor (FLY_BY / FLY_OVER) Replace static-radius nav_fw_wp_turn_smoothing with nav_fw_wp_turn_mode. FLY_BY anticipates the turn from the real coordinated-turn radius R = V^2/(g*tan(bank)) at d = R*tan(angle/2), so the turn starts at the correct distance at any speed; FLY_OVER flies over the WP then turns. Landing approach always uses FLY_BY. PG_NAV_CONFIG 7->8. --- docs/Settings.md | 9 ++-- src/main/cms/cms_menu_navigation.c | 2 +- src/main/fc/settings.yaml | 16 +++--- src/main/navigation/navigation.c | 20 ++++---- src/main/navigation/navigation.h | 9 ++-- src/main/navigation/navigation_fixedwing.c | 58 +++++++++++----------- 6 files changed, 57 insertions(+), 57 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index eaf52e6e8b6..24f6aa20a8b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4168,15 +4168,14 @@ Sets the maximum allowed alignment convergence angle to the waypoint course line --- -### nav_fw_wp_turn_smoothing +### nav_fw_wp_turn_mode -Smooths turns during WP missions by switching to a loiter turn at waypoints. When set to ON the craft will reach the waypoint during the turn. When set to ON-CUT the craft will turn inside the waypoint without actually reaching it (cuts the corner). +How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then turns onto the next leg. | Allowed Values | | | --- | --- | -| OFF | Default | -| ON | | -| ON-CUT | | +| FLY_BY | Default | +| FLY_OVER | | --- diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index f5b12301028..41e3ca59d7e 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -202,7 +202,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] = OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX), #endif OSD_SETTING_ENTRY("MISSION RESTART", SETTING_NAV_WP_MISSION_RESTART), - OSD_SETTING_ENTRY("WP TURN SMOOTHING", SETTING_NAV_FW_WP_TURN_SMOOTHING), + OSD_SETTING_ENTRY("WP TURN MODE", SETTING_NAV_FW_WP_TURN_MODE), OSD_SETTING_ENTRY("WP TRACKING ACCURACY", SETTING_NAV_FW_WP_TRACKING_ACCURACY), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5f6eae0cbbc..49d0352cc6b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -180,9 +180,9 @@ tables: - name: dynamic_gyro_notch_mode values: ["2D", "3D"] enum: dynamicGyroNotchMode_e - - name: nav_fw_wp_turn_smoothing - values: ["OFF", "ON", "ON-CUT"] - enum: wpFwTurnSmoothing_e + - name: nav_fw_wp_turn_mode + values: ["FLY_BY", "FLY_OVER"] + enum: navFwWpTurnMode_e - name: gps_auto_baud_max values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600'] enum: gpsBaudRate_e @@ -2670,11 +2670,11 @@ groups: field: fw.wp_tracking_max_angle min: 30 max: 80 - - name: nav_fw_wp_turn_smoothing - description: "Smooths turns during WP missions by switching to a loiter turn at waypoints. When set to ON the craft will reach the waypoint during the turn. When set to ON-CUT the craft will turn inside the waypoint without actually reaching it (cuts the corner)." - default_value: "OFF" - field: fw.wp_turn_smoothing - table: nav_fw_wp_turn_smoothing + - name: nav_fw_wp_turn_mode + description: "How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then turns onto the next leg." + default_value: "FLY_BY" + field: fw.wp_turn_mode + table: nav_fw_wp_turn_mode - name: nav_auto_speed description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]" default_value: 500 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a08d483d995..3cdf0fc51ef 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -250,7 +250,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT, // pitch angle mode deadband when Saoring mode enabled .wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions .wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs - .wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions + .wp_turn_mode = SETTING_NAV_FW_WP_TURN_MODE_DEFAULT, // FLY_BY, WP mission turn mode } ); @@ -3102,16 +3102,13 @@ bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBe posControl.wpDistance = calculateDistanceToDestination(waypointPos); // Check if waypoint was missed based on bearing to waypoint exceeding given angular limit relative to initial waypoint bearing. - // Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active + // Angular limit = 100 degs. uint16_t relativeBearingTargetAngle = 10000; if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) { - // If WP mode turn smoothing CUT option used waypoint is reached when start of turn is initiated - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) { - posControl.flags.wpTurnSmoothingActive = false; - return true; - } - relativeBearingTargetAngle = 6000; + // FLY_BY turn: the waypoint is reached when the anticipated corner-cut turn is initiated + posControl.flags.wpTurnSmoothingActive = false; + return true; } @@ -4303,7 +4300,8 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint) mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3)); calculateAndSetActiveWaypointToLocalPosition(&localPos); - if (navConfig()->fw.wp_turn_smoothing) { + // Turn anticipation (nextTurnAngle) is only needed for FLY_BY; FLY_OVER flies to the WP then turns. + if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_BY) { fpVector3_t posNextWp; if (getLocalPosNextWaypoint(&posNextWp)) { int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp); @@ -5547,7 +5545,9 @@ static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos { calculateAndSetActiveWaypointToLocalPosition(pos); - if (navConfig()->fw.wp_turn_smoothing && nextWpPos != NULL) { + // Landing approach always uses FLY_BY turns (clean cut onto the next approach leg), + // so the turn angle is set whenever a following approach waypoint exists. + if (nextWpPos != NULL) { int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, nextWpPos); posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing); } else { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 55c34dc6d64..5ca0a03a0a7 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -335,10 +335,9 @@ typedef enum { } rthTrackbackMode_e; typedef enum { - WP_TURN_SMOOTHING_OFF, - WP_TURN_SMOOTHING_ON, - WP_TURN_SMOOTHING_CUT, -} wpFwTurnSmoothing_e; + NAV_FW_WP_TURN_MODE_FLY_BY = 0, // corner cut: turn anticipated so the arc joins the next leg, WP passed abeam + NAV_FW_WP_TURN_MODE_FLY_OVER = 1, // fly over the WP, then turn onto the next leg +} navFwWpTurnMode_e; typedef enum { MC_ALT_HOLD_STICK, @@ -507,7 +506,7 @@ typedef struct navConfig_s { uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg) uint8_t wp_tracking_accuracy; // fixed wing tracking accuracy response factor uint8_t wp_tracking_max_angle; // fixed wing tracking accuracy max alignment angle [degs] - uint8_t wp_turn_smoothing; // WP mission turn smoothing options + uint8_t wp_turn_mode; // WP mission turn mode (navFwWpTurnMode_e: FLY_BY / FLY_OVER) } fw; } navConfig_t; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index c58bd00fc7b..93db0dfc327 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -70,6 +70,12 @@ // 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 +// FW waypoint turn predictor: clamps for the coordinated-turn radius used to time the FLY_BY turn +#define NAV_FW_TURN_MIN_SPEED 500.0f // [cm/s] speed floor for the radius calc (low-speed noise guard) +#define NAV_FW_TURN_RADIUS_MIN 1000.0f // [cm] 10 m lower clamp +#define NAV_FW_TURN_RADIUS_MAX 30000.0f // [cm] 300 m upper clamp (matches loiter_radius max) +#define NAV_FW_TURN_LEAD_TAN_MAX 3.7f // tan(half turn angle) cap (~150 deg) to bound the lead distance + // If this is enabled navigation won't be applied if velocity is below 3 m/s //#define NAV_FW_LIMIT_MIN_FLY_VELOCITY @@ -389,6 +395,17 @@ static float applyFwRollInSmoothing(float rollTargetCd, timeDelta_t deltaMicros, return out; } +// Predicted coordinated-turn radius [cm] for the current ground speed and the nav +// bank-angle limit: R = V^2 / (g * tan(phi)). Clamped to a sane range. Used to time +// the FLY_BY turn so it starts at the correct distance regardless of speed. +static float getFwCoordinatedTurnRadius(void) +{ + const float speed = MAX(posControl.actualState.velXY, NAV_FW_TURN_MIN_SPEED); // cm/s + const float bankRad = DEGREES_TO_RADIANS((float)navConfig()->fw.max_bank_angle); + const float radius = (speed * speed) / (GRAVITY_CMSS * tan_approx(bankRad)); // cm + return constrainf(radius, NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX); +} + 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) { @@ -421,37 +438,22 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) needToCalculateCircularLoiter = false; } - /* WP turn smoothing with 2 options, 1: pass through WP, 2: cut inside turn missing WP - * Works for turns > 30 degs and < 160 degs. - * Option 1 switches to loiter path around waypoint using navLoiterRadius. - * Loiter centered on point inside turn at required distance from waypoint and - * on a bearing midway between current and next waypoint course bearings. - * Option 2 simply uses a normal turn once the turn initiation point is reached */ + /* FLY_BY waypoint turn (corner cut): start the turn a geometric lead distance + * before the WP so the arc joins the next leg. The lead uses the live + * coordinated-turn radius and the true half-angle tangent, so the start point is + * correct at any speed (the old code used a fixed loiter radius and a clamped + * angle factor, which only matched one speed). nextTurnAngle is only set for + * FLY_BY waypoints and for landing approach, so this is skipped for FLY_OVER. + * Works for turns 30..160 deg. The precise turn arc itself is added with the + * feed-forward controller in a later PR; here only the timing is corrected. */ int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); posControl.flags.wpTurnSmoothingActive = false; if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - // turnStartFactor adjusts start of loiter based on turn angle - float turnStartFactor; - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { // passes through WP - turnStartFactor = waypointTurnAngle / 6000.0f; - } else { // // cut inside turn missing WP - turnStartFactor = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 1.0f, 2.0f); - } - // velXY provides additional turn initiation distance based on an assumed 1 second delayed turn response time - if (posControl.wpDistance < (posControl.actualState.velXY + navLoiterRadius * turnStartFactor)) { - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { - int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.bearing + 18000); - loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); - loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); - - posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; - posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; - - // turn direction to next waypoint - loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right - - needToCalculateCircularLoiter = true; - } + const float turnRadius = getFwCoordinatedTurnRadius(); + const float halfAngleTan = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 0.0f, NAV_FW_TURN_LEAD_TAN_MAX); + // velXY term is a ~1 s roll-in lead (replaced by a modelled roll-in in a later PR) + const float turnStartDistance = posControl.actualState.velXY + turnRadius * halfAngleTan; + if (posControl.wpDistance < turnStartDistance) { posControl.flags.wpTurnSmoothingActive = true; } } From 58e9783a2ab573369ee01dad634dca9fad515dc3 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 9 Jun 2026 18:35:51 +0200 Subject: [PATCH 06/21] FW nav: turn feed-forward, energy bank guard, loiter-radius floor, S-curve roll-in Builds on the FLY_BY/FLY_OVER turn predictor: - Energy/altitude bank guard (always-on): reduce the effective nav bank limit when a commanded climb cannot be sustained near the pitch/throttle limit while banked, from target-vs-actual vertical speed (filtered + Schmitt deadband + bank-entry baseline). Widens the turn/loiter so the climb recovers. - Loiter-radius floor: never demand a circle tighter than the effective bank allows. - Coordinated-turn feed-forward (nav_fw_turn_ff_gain, dev/experimental, default 100): command the geometric bank for the active turn/loiter radius so the PID only trims. - Roll-in S-curve replacing the control_smoothness PT1 (roll axis only): step-triggered, control_smoothness*50ms time constant, smoothstep then 1:1. Pitch PT1 retained. - DEBUG_FW_TURN channel for tuning. PG_NAV_CONFIG 8->9. (Includes minor comment trims to the B1 turn-predictor code per AGENT.md.) --- docs/Settings.md | 11 ++ src/main/build/debug.h | 1 + src/main/fc/cli.c | 3 +- src/main/fc/settings.yaml | 8 +- src/main/navigation/navigation.c | 1 + src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fixedwing.c | 179 +++++++++++++++++++-- 7 files changed, 187 insertions(+), 17 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 24f6aa20a8b..329070c9eff 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -795,6 +795,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) | LULU | | | SBUS2 | | | OSD_REFRESH | | +| FW_TURN | | --- @@ -4148,6 +4149,16 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within --- +### nav_fw_turn_ff_gain + +DEVELOPER/EXPERIMENTAL (to be hardcoded before release): turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn/loiter radius forward to the roll controller so the PID only trims the residual, giving cleaner coordinated turns and more precise loiter circles. 0 disables the feed-forward (pure PID). + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + ### nav_fw_wp_tracking_accuracy Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy. diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b33868af8b2..dbde5291924 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -80,6 +80,7 @@ typedef enum { DEBUG_LULU, DEBUG_SBUS2, DEBUG_OSD_REFRESH, + DEBUG_FW_TURN, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 4d0e5006506..289c9983b53 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -228,7 +228,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "GPS", "LULU", "SBUS2", - "OSD_REFRESH" + "OSD_REFRESH", + "FW_TURN" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 49d0352cc6b..c83a35d8baf 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,7 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", - "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH"] + "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "FW_TURN"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e @@ -2675,6 +2675,12 @@ groups: default_value: "FLY_BY" field: fw.wp_turn_mode table: nav_fw_wp_turn_mode + - name: nav_fw_turn_ff_gain + description: "DEVELOPER/EXPERIMENTAL (to be hardcoded before release): turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn/loiter radius forward to the roll controller so the PID only trims the residual, giving cleaner coordinated turns and more precise loiter circles. 0 disables the feed-forward (pure PID)." + default_value: 100 + field: fw.turn_ff_gain + min: 0 + max: 200 - name: nav_auto_speed description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]" default_value: 500 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 3cdf0fc51ef..b880bb52216 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -251,6 +251,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions .wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs .wp_turn_mode = SETTING_NAV_FW_WP_TURN_MODE_DEFAULT, // FLY_BY, WP mission turn mode + .turn_ff_gain = SETTING_NAV_FW_TURN_FF_GAIN_DEFAULT, // 0, turn FF off by default } ); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5ca0a03a0a7..d6a8944ffad 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -507,6 +507,7 @@ typedef struct navConfig_s { uint8_t wp_tracking_accuracy; // fixed wing tracking accuracy response factor uint8_t wp_tracking_max_angle; // fixed wing tracking accuracy max alignment angle [degs] uint8_t wp_turn_mode; // WP mission turn mode (navFwWpTurnMode_e: FLY_BY / FLY_OVER) + uint8_t turn_ff_gain; // turn coordination feed-forward gain [%] (0 = off; dev tuning, to be hardcoded) } fw; } navConfig_t; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 93db0dfc327..66bfb58e229 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -76,6 +76,23 @@ #define NAV_FW_TURN_RADIUS_MAX 30000.0f // [cm] 300 m upper clamp (matches loiter_radius max) #define NAV_FW_TURN_LEAD_TAN_MAX 3.7f // tan(half turn angle) cap (~150 deg) to bound the lead distance +// FW energy/altitude bank guard thresholds (conservative; observable via DEBUG_FW_TURN) +#define NAV_FW_GUARD_PHI_FLOOR_DEG 15.0f // minimum effective bank limit +#define NAV_FW_GUARD_MIN_BANK_DEG 10.0f // "banked" threshold +#define NAV_FW_GUARD_VZ_CLIMB_MIN 50.0f // only guard when commanding a climb above this +#define NAV_FW_GUARD_VZ_DEFICIT_ENTER 150.0f // filtered Vz deficit to start guarding (Schmitt high) +#define NAV_FW_GUARD_VZ_DEFICIT_EXIT 50.0f // filtered Vz deficit to stop guarding (Schmitt low) +#define NAV_FW_GUARD_VZ_FILTER_HZ 0.5f // deficit/rise low-pass cutoff (rejects wind/thermal/noise) +#define NAV_FW_GUARD_PITCH_FRAC 0.85f // "near max climb pitch" fraction +#define NAV_FW_GUARD_THROTTLE_MARGIN 50 // "near max throttle" margin +#define NAV_FW_GUARD_REDUCE_RATE_DPS 20.0f // bank-limit reduce rate +#define NAV_FW_GUARD_RECOVER_RATE_DPS 5.0f // bank-limit recover rate +#define NAV_FW_GUARD_RECOVER_HOLDOFF_MS 1000 // healthy time before recovery starts + +// Turn-coordination feed-forward: heading-error window over which the WP-turn FF tapers in (centideg) +#define NAV_FW_FF_HEADING_DEADBAND_CD 500.0f // below this heading error: no WP-turn FF +#define NAV_FW_FF_HEADING_FULL_CD 3000.0f // heading error for full WP-turn FF + // If this is enabled navigation won't be applied if velocity is below 3 m/s //#define NAV_FW_LIMIT_MIN_FLY_VELOCITY @@ -89,6 +106,8 @@ static bool fwRollSmoothReseed = false; // re-sync the roll S-curve smoother 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 float fwEffectiveBankLimit = 0.0f; // adaptive nav bank limit (energy guard), deg; 0 = not yet initialised +static float fwActiveLoiterRadius = 0.0f; // effective loiter radius in use (cm), for the turn feed-forward static int8_t loiterDirYaw = 1; static bool needToCalculateCircularLoiter; static bool autoSpeedIsActive = false; @@ -302,6 +321,9 @@ void resetFixedWingPositionController(void) fwRollSmoothSeedCd = ((micros() - fwLastNavRollCmdTimeUs) < MAX_POSITION_UPDATE_INTERVAL_US) ? fwLastNavRollCmdCd : 0.0f; fwRollSmoothReseed = true; + // Reset the energy-guard bank limit to the configured maximum (guard state re-syncs on next run) + fwEffectiveBankLimit = navConfig()->fw.max_bank_angle; + pt1FilterSetCutoff(&fwCrossTrackErrorRateFilterState, 3.0f); pt1FilterReset(&fwCrossTrackErrorRateFilterState, 0.0f); } @@ -395,9 +417,93 @@ static float applyFwRollInSmoothing(float rollTargetCd, timeDelta_t deltaMicros, return out; } -// Predicted coordinated-turn radius [cm] for the current ground speed and the nav -// bank-angle limit: R = V^2 / (g * tan(phi)). Clamped to a sane range. Used to time -// the FLY_BY turn so it starts at the correct distance regardless of speed. +// Effective nav bank limit [deg]: the energy guard's reduced limit, never above the configured max. +static float getFwEffectiveBankLimit(void) +{ + const float maxBank = (float)navConfig()->fw.max_bank_angle; + return (fwEffectiveBankLimit > 0.0f) ? MIN(fwEffectiveBankLimit, maxBank) : maxBank; +} + +// Reduce the effective bank limit when a commanded climb can't be sustained near the pitch/throttle +// limit while banked, so the turn/loiter widens and the climb recovers. Uses target-vs-actual Vz. +static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t correctedThrottleValue) +{ + static timeUs_t lastUpdateUs = 0; + static timeUs_t lastTriggerUs = 0; + static bool deficitLatched = false; + static bool bankedPrev = false; + static float targetVzBaseline = 0.0f; + static pt1Filter_t deficitFilter; + static pt1Filter_t riseFilter; + + const float maxBank = (float)navConfig()->fw.max_bank_angle; + if (fwEffectiveBankLimit <= 0.0f) { + fwEffectiveBankLimit = maxBank; + } + + const timeDeltaLarge_t dtUs = currentTimeUs - lastUpdateUs; + lastUpdateUs = currentTimeUs; + // First call / gap (controller was inactive): resync, skip integration this step. + if (dtUs <= 0 || dtUs > MAX_POSITION_UPDATE_INTERVAL_US) { + targetVzBaseline = posControl.desiredState.vel.z; + pt1FilterSetCutoff(&deficitFilter, NAV_FW_GUARD_VZ_FILTER_HZ); + pt1FilterSetCutoff(&riseFilter, NAV_FW_GUARD_VZ_FILTER_HZ); + pt1FilterReset(&deficitFilter, 0.0f); + pt1FilterReset(&riseFilter, 0.0f); + deficitLatched = false; + lastTriggerUs = currentTimeUs; + return; + } + const float dtSec = US2S(dtUs); + + const float bankDeg = fabsf((float)posControl.rcAdjustment[ROLL]) / 10.0f; // rcAdjustment is decidegrees + const bool banked = bankDeg > NAV_FW_GUARD_MIN_BANK_DEG; + + // Latch target Vz at bank entry as the pre-bank reference. + if (banked && !bankedPrev) { + targetVzBaseline = posControl.desiredState.vel.z; + } + bankedPrev = banked; + + const float targetVz = posControl.desiredState.vel.z; // cm/s + const float actualVz = navGetCurrentActualPositionAndVelocity()->vel.z; // cm/s + + // Signal A: unmet climb demand. Signal B: bank-induced rise of the demand since bank entry. + const float deficit = pt1FilterApply3(&deficitFilter, targetVz - actualVz, dtSec); + const float rise = pt1FilterApply3(&riseFilter, targetVz - targetVzBaseline, dtSec); + + // Schmitt trigger + deadband on the filtered deficit (rejects fluctuations). + if (deficit > NAV_FW_GUARD_VZ_DEFICIT_ENTER) { + deficitLatched = true; + } else if (deficit < NAV_FW_GUARD_VZ_DEFICIT_EXIT) { + deficitLatched = false; + } + + const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES((float)navConfig()->fw.max_climb_angle); + const bool nearPitchLimit = (float)posControl.rcAdjustment[PITCH] >= NAV_FW_GUARD_PITCH_FRAC * maxClimbDeciDeg; + const bool nearThrottleLimit = correctedThrottleValue >= (currentBatteryProfile->nav.fw.max_throttle - NAV_FW_GUARD_THROTTLE_MARGIN); + const bool climbCommanded = targetVz > NAV_FW_GUARD_VZ_CLIMB_MIN; + + const bool trigger = banked && climbCommanded && deficitLatched && (nearPitchLimit || nearThrottleLimit); + + // Bank-induced rise (signal B) -> react faster. + const float reduceRate = (rise > NAV_FW_GUARD_VZ_DEFICIT_ENTER) ? (2.0f * NAV_FW_GUARD_REDUCE_RATE_DPS) : NAV_FW_GUARD_REDUCE_RATE_DPS; + + if (trigger) { + fwEffectiveBankLimit -= reduceRate * dtSec; + lastTriggerUs = currentTimeUs; + } else if ((currentTimeUs - lastTriggerUs) > ((timeUs_t)NAV_FW_GUARD_RECOVER_HOLDOFF_MS * 1000)) { + fwEffectiveBankLimit += NAV_FW_GUARD_RECOVER_RATE_DPS * dtSec; + } + fwEffectiveBankLimit = constrainf(fwEffectiveBankLimit, NAV_FW_GUARD_PHI_FLOOR_DEG, maxBank); + + DEBUG_SET(DEBUG_FW_TURN, 1, lrintf(fwEffectiveBankLimit)); + DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(deficit)); + DEBUG_SET(DEBUG_FW_TURN, 5, lrintf(rise)); + DEBUG_SET(DEBUG_FW_TURN, 6, trigger); +} + +// Coordinated-turn radius R = V^2/(g*tan(phi)) [cm], clamped. Times the FLY_BY turn for any speed. static float getFwCoordinatedTurnRadius(void) { const float speed = MAX(posControl.actualState.velXY, NAV_FW_TURN_MIN_SPEED); // cm/s @@ -406,6 +512,34 @@ static float getFwCoordinatedTurnRadius(void) return constrainf(radius, NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX); } +// Coordinated-turn feed-forward bank [centideg] for the active loiter/WP turn (0 if disabled or straight). +static float getFwTurnFeedForward(int32_t navHeadingError) +{ + const uint8_t ffGain = navConfig()->fw.turn_ff_gain; + if (ffGain == 0 || posControl.actualState.velXY <= NAV_FW_TURN_MIN_SPEED) { + return 0.0f; + } + + float ffRadius = 0.0f; + float ffSign = 0.0f; + if (needToCalculateCircularLoiter) { // loiter circle: known radius + ffRadius = fwActiveLoiterRadius; + ffSign = (float)loiterDirection(); + } else if (isWaypointNavTrackingActive() && ABS(navHeadingError) > NAV_FW_FF_HEADING_DEADBAND_CD) { + ffRadius = getFwCoordinatedTurnRadius(); // WP turn: dynamic radius, tapered by heading error + ffSign = (navHeadingError > 0 ? 1.0f : -1.0f) * constrainf((float)ABS(navHeadingError) / NAV_FW_FF_HEADING_FULL_CD, 0.0f, 1.0f); + } + + float rollFF = 0.0f; + if (ffRadius > 0.0f) { + const float v = posControl.actualState.velXY; + const float phiFFcd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * ffRadius))); + rollFF = ffSign * phiFFcd * (ffGain / 100.0f); + } + DEBUG_SET(DEBUG_FW_TURN, 7, lrintf(rollFF)); + return rollFF; +} + 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) { @@ -421,6 +555,16 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius); + + /* Floor the loiter radius to what the (possibly guard-reduced) bank limit can fly at this speed, + * so the aircraft holds a stable circle instead of chasing an unachievable one. */ + { + const float speed = MAX(posControl.actualState.velXY, NAV_FW_TURN_MIN_SPEED); + const float minRadius = (speed * speed) / (GRAVITY_CMSS * tan_approx(DEGREES_TO_RADIANS(getFwEffectiveBankLimit()))); + navLoiterRadius = MAX(navLoiterRadius, (uint32_t)constrainf(minRadius, NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX)); + } + fwActiveLoiterRadius = (float)navLoiterRadius; // expose to the turn feed-forward + fpVector3_t loiterCenterPos = posControl.desiredState.pos; int8_t loiterTurnDirection = loiterDirection(); @@ -438,14 +582,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) needToCalculateCircularLoiter = false; } - /* FLY_BY waypoint turn (corner cut): start the turn a geometric lead distance - * before the WP so the arc joins the next leg. The lead uses the live - * coordinated-turn radius and the true half-angle tangent, so the start point is - * correct at any speed (the old code used a fixed loiter radius and a clamped - * angle factor, which only matched one speed). nextTurnAngle is only set for - * FLY_BY waypoints and for landing approach, so this is skipped for FLY_OVER. - * Works for turns 30..160 deg. The precise turn arc itself is added with the - * feed-forward controller in a later PR; here only the timing is corrected. */ + /* FLY_BY corner cut: start the turn R*tan(angle/2) before the WP so the arc joins the next leg + * at any speed. Only runs when nextTurnAngle is set (FLY_BY waypoints + landing); FLY_OVER skips it. */ int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); posControl.flags.wpTurnSmoothingActive = false; if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { @@ -453,6 +591,9 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) const float halfAngleTan = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 0.0f, NAV_FW_TURN_LEAD_TAN_MAX); // velXY term is a ~1 s roll-in lead (replaced by a modelled roll-in in a later PR) const float turnStartDistance = posControl.actualState.velXY + turnRadius * halfAngleTan; + DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(turnRadius)); + DEBUG_SET(DEBUG_FW_TURN, 2, lrintf(turnStartDistance)); + DEBUG_SET(DEBUG_FW_TURN, 3, lrintf(posControl.wpDistance)); if (posControl.wpDistance < turnStartDistance) { posControl.flags.wpTurnSmoothingActive = true; } @@ -627,15 +768,20 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); // Input error in (deg*100), output roll angle (deg*100) + const float navBankLimit = getFwEffectiveBankLimit(); // energy-guard reduced limit (<= nav_fw_bank_angle) float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, US2S(deltaMicros), - -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), - DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), + -DEGREES_TO_CENTIDEGREES(navBankLimit), + DEGREES_TO_CENTIDEGREES(navBankLimit), pidFlags); + // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. + rollAdjustment += getFwTurnFeedForward(navHeadingError); + // 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; + rollAdjustment = constrainf(rollAdjustment, -DEGREES_TO_CENTIDEGREES(navBankLimit), DEGREES_TO_CENTIDEGREES(navBankLimit)); // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees) posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment); @@ -758,8 +904,8 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) { // ROLL >0 right, <0 left - const uint8_t maxBankAngle = navConfig()->fw.max_bank_angle; - int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -DEGREES_TO_DECIDEGREES(maxBankAngle), DEGREES_TO_DECIDEGREES(maxBankAngle)); + const int16_t navBankLimitDeciDeg = (int16_t)lrintf(DEGREES_TO_DECIDEGREES(getFwEffectiveBankLimit())); + int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -navBankLimitDeciDeg, navBankLimitDeciDeg); rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]); } @@ -808,6 +954,9 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat } rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false); + + // Update the energy guard now that this cycle's pitch + throttle commands are known. + updateFwEnergyBankGuard(currentTimeUs, correctedThrottleValue); } } From 2496dacb79047d767cd897bb8d93129a12734553 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Wed, 10 Jun 2026 11:37:57 +0200 Subject: [PATCH 07/21] FW nav: stabilised loiter radius + target/ceiling bank semantics + loiter-only headroom Refinements on the B2 feed-forward/guard work (navigation_fixedwing.c): - Loiter-radius floor -> stabilised per-revolution peak hold with 1 m/s gradual decay (getFwStableLoiterRadius): ratchet up at once, hold the peak over a full revolution (orbital azimuth net 360deg), ease down at <=1 m/s. Stops the commanded circle thrashing with wind-driven ground-speed swings. - Bank-limit semantics: nav_fw_bank_angle is the planning TARGET; control output may use reserve up to the hard ceiling max_angle_inclination_rll to hold the radius (getFwBankCeilingDeg / getFwEffectiveBankLimit / getFwPlanningBankDeg). Energy guard reduces the ceiling and snaps straight to the target on trigger. - Headroom is loiter-only (getFwControlBankLimit): WP turns/cruise clamp to the planning target so coordinated turns fly a clean arc, not the hard ceiling. - Roll-in S-curve time constant -> control_smoothness*100ms (cap 1000ms). --- src/main/navigation/navigation_fixedwing.c | 120 +++++++++++++++++---- 1 file changed, 101 insertions(+), 19 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 66bfb58e229..a4a48407066 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -74,6 +74,7 @@ #define NAV_FW_TURN_MIN_SPEED 500.0f // [cm/s] speed floor for the radius calc (low-speed noise guard) #define NAV_FW_TURN_RADIUS_MIN 1000.0f // [cm] 10 m lower clamp #define NAV_FW_TURN_RADIUS_MAX 30000.0f // [cm] 300 m upper clamp (matches loiter_radius max) +#define NAV_FW_LOITER_RADIUS_DECAY 100.0f // [cm/s] max rate the held loiter radius eases back down (1 m/s) #define NAV_FW_TURN_LEAD_TAN_MAX 3.7f // tan(half turn angle) cap (~150 deg) to bound the lead distance // FW energy/altitude bank guard thresholds (conservative; observable via DEBUG_FW_TURN) @@ -321,8 +322,8 @@ void resetFixedWingPositionController(void) fwRollSmoothSeedCd = ((micros() - fwLastNavRollCmdTimeUs) < MAX_POSITION_UPDATE_INTERVAL_US) ? fwLastNavRollCmdCd : 0.0f; fwRollSmoothReseed = true; - // Reset the energy-guard bank limit to the configured maximum (guard state re-syncs on next run) - fwEffectiveBankLimit = navConfig()->fw.max_bank_angle; + // Reset the energy-guard bank limit; 0 = use full ceiling until the guard re-syncs on its next run + fwEffectiveBankLimit = 0.0f; pt1FilterSetCutoff(&fwCrossTrackErrorRateFilterState, 3.0f); pt1FilterReset(&fwCrossTrackErrorRateFilterState, 0.0f); @@ -417,11 +418,34 @@ static float applyFwRollInSmoothing(float rollTargetCd, timeDelta_t deltaMicros, return out; } -// Effective nav bank limit [deg]: the energy guard's reduced limit, never above the configured max. +// Hard roll ceiling [deg] = the global angle-mode limit (max_angle_inclination_rll); the nav control +// output may never exceed it (also enforced downstream by pidAngleToRcCommand). +static float getFwBankCeilingDeg(void) +{ + return (float)pidProfile()->max_angle_inclination[FD_ROLL] / 10.0f; +} + +// Control-output bank ceiling [deg]: the hard roll ceiling, reduced by the energy guard. Roll PID/FF +// corrections may climb to here to HOLD the radius; nav_fw_bank_angle is only the planning target. static float getFwEffectiveBankLimit(void) { - const float maxBank = (float)navConfig()->fw.max_bank_angle; - return (fwEffectiveBankLimit > 0.0f) ? MIN(fwEffectiveBankLimit, maxBank) : maxBank; + const float ceiling = getFwBankCeilingDeg(); + return (fwEffectiveBankLimit > 0.0f) ? MIN(fwEffectiveBankLimit, ceiling) : ceiling; +} + +// Planning bank [deg] for sizing turn/loiter radii: nav_fw_bank_angle as the TARGET, capped by the +// (guard-reduced) ceiling. If nav_fw_bank_angle >= the ceiling, planning == ceiling (hard limit). +static float getFwPlanningBankDeg(void) +{ + return MIN((float)navConfig()->fw.max_bank_angle, getFwEffectiveBankLimit()); +} + +// Roll-command bank limit [deg]: in a held loiter the controller may use the reserve up to the guard +// ceiling to reject wind and hold the circle; everywhere else (WP turns, cruise) it stays at the +// planning target so coordinated turns fly a clean arc at nav_fw_bank_angle, not the hard ceiling. +static float getFwControlBankLimit(void) +{ + return (navGetCurrentStateFlags() & NAV_CTL_HOLD) ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); } // Reduce the effective bank limit when a commanded climb can't be sustained near the pitch/throttle @@ -436,7 +460,8 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t correctedTh static pt1Filter_t deficitFilter; static pt1Filter_t riseFilter; - const float maxBank = (float)navConfig()->fw.max_bank_angle; + const float maxBank = getFwBankCeilingDeg(); + const float targetBank = MIN((float)navConfig()->fw.max_bank_angle, maxBank); // planning target = guard snap level if (fwEffectiveBankLimit <= 0.0f) { fwEffectiveBankLimit = maxBank; } @@ -490,7 +515,8 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t correctedTh const float reduceRate = (rise > NAV_FW_GUARD_VZ_DEFICIT_ENTER) ? (2.0f * NAV_FW_GUARD_REDUCE_RATE_DPS) : NAV_FW_GUARD_REDUCE_RATE_DPS; if (trigger) { - fwEffectiveBankLimit -= reduceRate * dtSec; + fwEffectiveBankLimit = MIN(fwEffectiveBankLimit, targetBank); // drop headroom at once: snap to the planning target + fwEffectiveBankLimit -= reduceRate * dtSec; // then keep easing down toward the floor lastTriggerUs = currentTimeUs; } else if ((currentTimeUs - lastTriggerUs) > ((timeUs_t)NAV_FW_GUARD_RECOVER_HOLDOFF_MS * 1000)) { fwEffectiveBankLimit += NAV_FW_GUARD_RECOVER_RATE_DPS * dtSec; @@ -507,7 +533,7 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t correctedTh static float getFwCoordinatedTurnRadius(void) { const float speed = MAX(posControl.actualState.velXY, NAV_FW_TURN_MIN_SPEED); // cm/s - const float bankRad = DEGREES_TO_RADIANS((float)navConfig()->fw.max_bank_angle); + const float bankRad = DEGREES_TO_RADIANS(getFwPlanningBankDeg()); // plan for the target bank const float radius = (speed * speed) / (GRAVITY_CMSS * tan_approx(bankRad)); // cm return constrainf(radius, NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX); } @@ -540,7 +566,65 @@ static float getFwTurnFeedForward(int32_t navHeadingError) return rollFF; } -static void calculateVirtualPositionTarget_FW(float trackingPeriod) +// Loiter-radius floor [cm], stabilised. The tightest holdable circle scales with ground-speed squared, +// so it swings with wind; commanding that every loop makes the fixed-radius loiter tracker thrash. We +// ratchet UP immediately (safety), hold the PEAK over a full revolution, then ease DOWN toward that +// revolution's peak at <= NAV_FW_LOITER_RADIUS_DECAY (no abrupt drop after a gust). Revolution = the +// aircraft's azimuth about the loiter centre sweeping a net 360deg (heading-independent). +static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearingFromCenterRad, bool loiterActive, timeDelta_t deltaMicros) +{ + static bool active = false; + static float commandedHold = 0.0f; + static float decayTarget = 0.0f; + static float revPeak = 0.0f; + static float netAngle = 0.0f; + static float prevBearing = 0.0f; + + const float speed = MAX(posControl.actualState.velXY, NAV_FW_TURN_MIN_SPEED); + const float required = constrainf((speed * speed) / (GRAVITY_CMSS * tan_approx(DEGREES_TO_RADIANS(getFwPlanningBankDeg()))), + NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX); + + if (!loiterActive) { + active = false; + commandedHold = required; // transit / WP turn: track instantaneously + } else { + if (!active) { // loiter entry: seed (no decay until the first revolution) + active = true; + commandedHold = required; + decayTarget = NAV_FW_TURN_RADIUS_MAX; + revPeak = required; + netAngle = 0.0f; + prevBearing = bearingFromCenterRad; + } + revPeak = MAX(revPeak, required); + commandedHold = MAX(commandedHold, required); // ratchet up immediately (safety) + + float dAng = bearingFromCenterRad - prevBearing; + if (dAng > M_PIf) dAng -= 2.0f * M_PIf; + if (dAng < -M_PIf) dAng += 2.0f * M_PIf; + netAngle += dAng; // signed net rotation about the centre + prevBearing = bearingFromCenterRad; + + if (fabsf(netAngle) >= 2.0f * M_PIf) { // a full revolution -> this revolution's peak is the decay target + decayTarget = revPeak; + revPeak = required; + netAngle = 0.0f; + } + + if (commandedHold > decayTarget) { // ease down gradually, never below the current need + commandedHold -= NAV_FW_LOITER_RADIUS_DECAY * US2S(deltaMicros); + commandedHold = MAX(commandedHold, MAX(decayTarget, required)); + } + } + + const uint32_t out = (uint32_t)MAX((float)configuredRadius, commandedHold); + DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(out)); // commanded loiter radius (stabilised) + DEBUG_SET(DEBUG_FW_TURN, 2, lrintf(required)); // instantaneous required (swings with wind) + DEBUG_SET(DEBUG_FW_TURN, 3, lrintf(speed)); // ground speed + return out; +} + +static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t deltaMicros) { if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) { return; @@ -556,13 +640,11 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius); - /* Floor the loiter radius to what the (possibly guard-reduced) bank limit can fly at this speed, - * so the aircraft holds a stable circle instead of chasing an unachievable one. */ - { - const float speed = MAX(posControl.actualState.velXY, NAV_FW_TURN_MIN_SPEED); - const float minRadius = (speed * speed) / (GRAVITY_CMSS * tan_approx(DEGREES_TO_RADIANS(getFwEffectiveBankLimit()))); - navLoiterRadius = MAX(navLoiterRadius, (uint32_t)constrainf(minRadius, NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX)); - } + /* Loiter-radius floor with per-revolution peak hold (see getFwStableLoiterRadius): keep the circle + * stable for the fixed-radius loiter tracker instead of chasing the wind-varying instantaneous value. */ + const bool inLoiter = (navGetCurrentStateFlags() & NAV_CTL_HOLD); + const float bearingFromCenter = atan2_approx(-posErrorY, -posErrorX); + navLoiterRadius = getFwStableLoiterRadius(navLoiterRadius, bearingFromCenter, inLoiter, deltaMicros); fwActiveLoiterRadius = (float)navLoiterRadius; // expose to the turn feed-forward fpVector3_t loiterCenterPos = posControl.desiredState.pos; @@ -768,7 +850,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); // Input error in (deg*100), output roll angle (deg*100) - const float navBankLimit = getFwEffectiveBankLimit(); // energy-guard reduced limit (<= nav_fw_bank_angle) + const float navBankLimit = getFwControlBankLimit(); // planning target on WP turns, guard ceiling in loiter float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, US2S(deltaMicros), -DEGREES_TO_CENTIDEGREES(navBankLimit), DEGREES_TO_CENTIDEGREES(navBankLimit), @@ -815,7 +897,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs) // Account for pilot's roll input (move position target left/right at max of max_manual_speed) // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs // FIXME: verify the above - calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2); + calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2, deltaMicrosPositionUpdate); updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate); needToCalculateCircularLoiter = false; } @@ -904,7 +986,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) { // ROLL >0 right, <0 left - const int16_t navBankLimitDeciDeg = (int16_t)lrintf(DEGREES_TO_DECIDEGREES(getFwEffectiveBankLimit())); + const int16_t navBankLimitDeciDeg = (int16_t)lrintf(DEGREES_TO_DECIDEGREES(getFwControlBankLimit())); int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -navBankLimitDeciDeg, navBankLimitDeciDeg); rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]); } From 42919384d7365231e3848f023c0d5ea9c4912eaa Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 11 Jun 2026 15:08:08 +0200 Subject: [PATCH 08/21] FW nav: arc-based coordinated WP turn coordinator Replace the heading-PID corner turn with an explicit coordinated arc on real WP-to-WP turns (>30 deg). New nav_fw_wp_turn_coordination = COORDINATED (default) / DIRECT (legacy fallback). The turn is a variable-radius spline driven directly on the roll axis: - RAMP_IN: smoothstep bank 0->phi_nom (no servo slam; control_smoothness folded into the ramp time and bypassed during the arc). - STEADY: direct radius control (nominal + radial pull-back + tangent alignment) against an inscribed circle placed tangent to BOTH legs, so the exit lands on the out-leg instead of offset. - CAPTURE: closed-loop roll-out, bank proportional to the heading still to go -> levels exactly on the out-leg, cannot overshoot the heading. Roll-aware easing: ease time = 1.5*phi/roll_rate + control_smoothness + the new nav_fw_wp_turn_control_ease (servo/inertia margin); the FLY_BY turn-start lead is sized from it so the longer eased path still starts in time. PG_NAV_CONFIG 10 -> 11. Settings: nav_fw_wp_turn_coordination, nav_fw_wp_turn_handback_angle, nav_fw_wp_turn_max_lead_time, nav_fw_wp_turn_control_ease (all dev/experimental). --- docs/Settings.md | 41 ++++ src/main/fc/settings.yaml | 26 +++ src/main/navigation/navigation.c | 8 +- src/main/navigation/navigation.h | 9 + src/main/navigation/navigation_fixedwing.c | 216 +++++++++++++++++++-- 5 files changed, 280 insertions(+), 20 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 329070c9eff..ea70d02ebeb 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4179,6 +4179,47 @@ Sets the maximum allowed alignment convergence angle to the waypoint course line --- +### nav_fw_wp_turn_control_ease + +DEVELOPER/EXPERIMENTAL: unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramp; SIM low, real models higher. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 500 | + +--- + +### nav_fw_wp_turn_coordination + +How FW waypoint turns are flown. COORDINATED (default) commands an explicit coordinated arc of the planned radius at nav_fw_bank_angle and tracks it to the next leg. DIRECT uses the legacy heading-PID turn (fallback for users who prefer the old behaviour). + +| Allowed Values | | +| --- | --- | +| DIRECT | | +| COORDINATED | Default | + +--- + +### nav_fw_wp_turn_handback_angle + +DEVELOPER/EXPERIMENTAL (to be hardcoded before release): heading error to the next leg [deg] at which the arc turn coordinator hands control back to the normal heading PID. + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 5 | 45 | + +--- + +### nav_fw_wp_turn_max_lead_time + +DEVELOPER/EXPERIMENTAL (to be hardcoded before release): FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large angles; above this the turn waits, then flies a non-tangent recovery. + +| Default | Min | Max | +| --- | --- | --- | +| 3000 | 0 | 10000 | + +--- + ### nav_fw_wp_turn_mode How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then turns onto the next leg. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c83a35d8baf..c546ab8bc10 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -183,6 +183,9 @@ tables: - name: nav_fw_wp_turn_mode values: ["FLY_BY", "FLY_OVER"] enum: navFwWpTurnMode_e + - name: nav_fw_wp_turn_coordination + values: ["DIRECT", "COORDINATED"] + enum: navFwWpTurnCoordination_e - name: gps_auto_baud_max values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600'] enum: gpsBaudRate_e @@ -2681,6 +2684,29 @@ groups: field: fw.turn_ff_gain min: 0 max: 200 + - name: nav_fw_wp_turn_coordination + description: "How FW waypoint turns are flown. COORDINATED (default) commands an explicit coordinated arc of the planned radius at nav_fw_bank_angle and tracks it to the next leg. DIRECT uses the legacy heading-PID turn (fallback for users who prefer the old behaviour)." + default_value: "COORDINATED" + field: fw.wp_turn_coordination + table: nav_fw_wp_turn_coordination + - name: nav_fw_wp_turn_handback_angle + description: "DEVELOPER/EXPERIMENTAL (to be hardcoded before release): heading error to the next leg [deg] at which the arc turn coordinator hands control back to the normal heading PID." + default_value: 15 + field: fw.wp_turn_handback_angle + min: 5 + max: 45 + - name: nav_fw_wp_turn_max_lead_time + description: "DEVELOPER/EXPERIMENTAL (to be hardcoded before release): FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large angles; above this the turn waits, then flies a non-tangent recovery." + default_value: 3000 + field: fw.wp_turn_max_lead_time + min: 0 + max: 10000 + - name: nav_fw_wp_turn_control_ease + description: "DEVELOPER/EXPERIMENTAL: unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramp; SIM low, real models higher." + default_value: 100 + field: fw.wp_turn_control_ease + min: 0 + max: 500 - name: nav_auto_speed description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]" default_value: 500 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b880bb52216..628e165f96e 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, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 11); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -251,7 +251,11 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions .wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs .wp_turn_mode = SETTING_NAV_FW_WP_TURN_MODE_DEFAULT, // FLY_BY, WP mission turn mode - .turn_ff_gain = SETTING_NAV_FW_TURN_FF_GAIN_DEFAULT, // 0, turn FF off by default + .turn_ff_gain = SETTING_NAV_FW_TURN_FF_GAIN_DEFAULT, // 100, turn FF + .wp_turn_coordination = SETTING_NAV_FW_WP_TURN_COORDINATION_DEFAULT, // COORDINATED, arc-based turns + .wp_turn_handback_angle = SETTING_NAV_FW_WP_TURN_HANDBACK_ANGLE_DEFAULT, // 15 deg + .wp_turn_max_lead_time = SETTING_NAV_FW_WP_TURN_MAX_LEAD_TIME_DEFAULT, // 3000 ms + .wp_turn_control_ease = SETTING_NAV_FW_WP_TURN_CONTROL_EASE_DEFAULT, // 100 ms } ); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d6a8944ffad..71ba9d31a61 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -339,6 +339,11 @@ typedef enum { NAV_FW_WP_TURN_MODE_FLY_OVER = 1, // fly over the WP, then turn onto the next leg } navFwWpTurnMode_e; +typedef enum { + NAV_FW_WP_TURN_DIRECT = 0, // legacy heading-PID turn (fallback) + NAV_FW_WP_TURN_COORDINATED = 1, // arc-based coordinated turn (default) +} navFwWpTurnCoordination_e; + typedef enum { MC_ALT_HOLD_STICK, MC_ALT_HOLD_MID, @@ -508,6 +513,10 @@ typedef struct navConfig_s { uint8_t wp_tracking_max_angle; // fixed wing tracking accuracy max alignment angle [degs] uint8_t wp_turn_mode; // WP mission turn mode (navFwWpTurnMode_e: FLY_BY / FLY_OVER) uint8_t turn_ff_gain; // turn coordination feed-forward gain [%] (0 = off; dev tuning, to be hardcoded) + uint8_t wp_turn_coordination; // turn handling (navFwWpTurnCoordination_e: DIRECT / COORDINATED) + uint8_t wp_turn_handback_angle; // arc -> direct PID handback heading error [deg] (dev tuning) + uint16_t wp_turn_max_lead_time; // FLY_BY: cap on how early the turn may start before the WP [ms] (dev tuning) + uint16_t wp_turn_control_ease; // unmodelled roll-response lag added to the computed turn ease time [ms] } fw; } navConfig_t; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index a4a48407066..81152a399c9 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -76,6 +76,12 @@ #define NAV_FW_TURN_RADIUS_MAX 30000.0f // [cm] 300 m upper clamp (matches loiter_radius max) #define NAV_FW_LOITER_RADIUS_DECAY 100.0f // [cm/s] max rate the held loiter radius eases back down (1 m/s) #define NAV_FW_TURN_LEAD_TAN_MAX 3.7f // tan(half turn angle) cap (~150 deg) to bound the lead distance +#define NAV_FW_ARC_MIN_TURN_ANGLE_CD 3000 // [centideg] only fly the coordinated arc for turns sharper than 30 deg +#define NAV_FW_ARC_HANDBACK_GUARD_MS 300.0f // [ms] min time in the arc before handback may fire (anti early-handback) +#define NAV_FW_ARC_RADIAL_GAIN 0.5f // [centideg bank / cm radial error] pull back onto the arc radius (TBD from flight) +#define NAV_FW_ARC_HEADING_GAIN 0.3f // [centideg bank / centideg tangent heading error] align to the arc (TBD from flight) +#define NAV_FW_ARC_EXIT_GAIN 2.0f // [centideg bank / centideg heading error] proportional roll-out capture: bank -> 0 as cog reaches the out-leg (no overshoot) +#define NAV_FW_ARC_EXIT_HANDOFF_CD 150 // [centideg] hand back to the PID within this heading error of the out-leg (keep low: residual bank = gain*this) // FW energy/altitude bank guard thresholds (conservative; observable via DEBUG_FW_TURN) #define NAV_FW_GUARD_PHI_FLOOR_DEG 15.0f // minimum effective bank limit @@ -109,6 +115,10 @@ static float fwLastNavRollCmdCd = 0.0f; // last applied nav roll command [ce static timeUs_t fwLastNavRollCmdTimeUs = 0; // nav-to-nav transition apart from a pilot handover at reset time static float fwEffectiveBankLimit = 0.0f; // adaptive nav bank limit (energy guard), deg; 0 = not yet initialised static float fwActiveLoiterRadius = 0.0f; // effective loiter radius in use (cm), for the turn feed-forward +static bool fwArcActive = false; // arc turn coordinator is driving the turn (-> bank headroom, suppress cross-track, roll override) +static bool fwFlyByCappedLatch = false; // the pending FLY_BY turn hit the lead-time cap -> fly it direct, not as an arc +static int8_t fwArcDir = 1; // active arc turn direction (+1 right / -1 left) +static float fwArcBankCmd = 0.0f; // direct-radius arc bank command [centideg] (Approach B), applied to roll while fwArcActive static int8_t loiterDirYaw = 1; static bool needToCalculateCircularLoiter; static bool autoSpeedIsActive = false; @@ -308,6 +318,8 @@ void resetFixedWingPositionController(void) virtualDesiredPosition.x = 0; virtualDesiredPosition.y = 0; virtualDesiredPosition.z = 0; + fwArcActive = false; + fwFlyByCappedLatch = false; navPidReset(&posControl.pids.fw_nav); navPidReset(&posControl.pids.fw_heading); @@ -440,12 +452,13 @@ static float getFwPlanningBankDeg(void) return MIN((float)navConfig()->fw.max_bank_angle, getFwEffectiveBankLimit()); } -// Roll-command bank limit [deg]: in a held loiter the controller may use the reserve up to the guard -// ceiling to reject wind and hold the circle; everywhere else (WP turns, cruise) it stays at the -// planning target so coordinated turns fly a clean arc at nav_fw_bank_angle, not the hard ceiling. +// Roll-command bank limit [deg]: a held loiter OR an active arc may use the reserve up to the guard +// ceiling — the loiter to hold its circle, the arc's radial term to pull back onto the radius against +// wind. (Approach B commands the bank directly with no lead bias, so the reserve is used only for +// genuine radial error, never over-banked.) Everywhere else (direct/capped/shallow turns, cruise) = target. static float getFwControlBankLimit(void) { - return (navGetCurrentStateFlags() & NAV_CTL_HOLD) ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); + return ((navGetCurrentStateFlags() & NAV_CTL_HOLD) || fwArcActive) ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); } // Reduce the effective bank limit when a commanded climb can't be sustained near the pitch/throttle @@ -524,9 +537,7 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t correctedTh fwEffectiveBankLimit = constrainf(fwEffectiveBankLimit, NAV_FW_GUARD_PHI_FLOOR_DEG, maxBank); DEBUG_SET(DEBUG_FW_TURN, 1, lrintf(fwEffectiveBankLimit)); - DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(deficit)); - DEBUG_SET(DEBUG_FW_TURN, 5, lrintf(rise)); - DEBUG_SET(DEBUG_FW_TURN, 6, trigger); + // ch4/5/6 temporarily owned by the arc coordinator diagnostic (guard deficit/rise/trigger muted) } // Coordinated-turn radius R = V^2/(g*tan(phi)) [cm], clamped. Times the FLY_BY turn for any speed. @@ -548,7 +559,10 @@ static float getFwTurnFeedForward(int32_t navHeadingError) float ffRadius = 0.0f; float ffSign = 0.0f; - if (needToCalculateCircularLoiter) { // loiter circle: known radius + if (fwArcActive) { // arc turn: full coordinated bank, NO taper (the arc, not the heading error, sets the bank) + ffRadius = getFwCoordinatedTurnRadius(); + ffSign = (float)fwArcDir; + } else if (needToCalculateCircularLoiter) { // loiter circle: known radius ffRadius = fwActiveLoiterRadius; ffSign = (float)loiterDirection(); } else if (isWaypointNavTrackingActive() && ABS(navHeadingError) > NAV_FW_FF_HEADING_DEADBAND_CD) { @@ -624,6 +638,148 @@ static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearing return out; } +// Modelled roll-in/out ease time [ms]: how long to ramp the bank to phiNom (and back). Roll-rate floor +// (1.5x so a smoothstep ramp's peak rate == roll_rate), the control_smoothness window folded in (gentleness +// / structural protection), plus the user's unmodelled servo+inertia margin. CS is bypassed during the arc, +// so this is the single, deterministic source of the turn's roll dynamics. +static float fwTurnEaseTimeMs(float phiNomDeg) +{ + const float rollRateDps = currentControlProfile->stabilized.rates[FD_ROLL] * 10.0f; + const float rollMs = (rollRateDps > 1.0f) ? (1.5f * phiNomDeg / rollRateDps * 1000.0f) : 0.0f; + const float csMs = MIN((float)navConfig()->fw.control_smoothness * NAV_FW_SMOOTH_TCONST_PER_STEP_MS, NAV_FW_SMOOTH_TCONST_MAX_MS); + return rollMs + csMs + (float)navConfig()->fw.wp_turn_control_ease; +} + +// Arc-based turn coordinator (Approach B + roll-aware easing): on a real WP-to-WP turn (course change > 30 deg) +// fly a variable-radius spline = smoothstep bank ramp 0->phiNom (RAMP_IN) -> coordinated arc (STEADY, direct +// radius control: nominal + radial pull-back kR + tangent alignment kH) -> ramp phiNom->0 (RAMP_OUT) -> hand +// back to the PID level + aligned. The ramps make the command achievable (no slam, no roll-in drift); the +// entry is anticipated geometrically by the FLY_BY turnStartDistance. Sets fwArcActive (drives roll directly). +static void updateFwTurnArc(timeDelta_t deltaMicros) +{ + enum { ARC_RAMP_IN = 0, ARC_STEADY, ARC_CAPTURE }; + static bool active = false; + static uint8_t phase; + static int32_t prevLegBearing = -1; + static float arcCx, arcCy, arcR; + static int8_t arcDir; + static int32_t arcOutBearing; + static float phiNomCd; // coordinated nominal bank for this turn [centideg] + static float tEaseMs; // roll-in ease time + static float rampMs; // elapsed time in the ramp-in phase + + fwArcActive = false; + + const bool wpTracking = isWaypointNavTrackingActive() && !needToCalculateCircularLoiter; + if (navConfig()->fw.wp_turn_coordination != NAV_FW_WP_TURN_COORDINATED || !wpTracking) { + active = false; + prevLegBearing = -1; + return; + } + + const int32_t legBearing = posControl.activeWaypoint.bearing; + const int32_t cog = posControl.actualState.cog; + const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; + const float v = posControl.actualState.velXY; + + if (!active) { + const bool legChanged = (prevLegBearing >= 0) && (ABS(wrap_18000(legBearing - prevLegBearing)) > 500); + prevLegBearing = legBearing; + if (legChanged) { + const bool capped = fwFlyByCappedLatch; // a capped FLY_BY turn is flown direct, not as an arc + fwFlyByCappedLatch = false; // consume the latch on any leg change + const int32_t hdgErr = wrap_18000(legBearing - cog); + if (!capped && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + const float arcRtmp = getFwCoordinatedTurnRadius(); + const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); + const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); + const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / arcRtmp)); // v/R == g*tan(phi)/v + const float psiTmp = 0.5f * omegaNomCds * (tTmp / 1000.0f); + if (2.0f * psiTmp < (float)ABS(hdgErr)) { // enough turn left for a steady arc between the ease ramps + active = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + arcR = arcRtmp; + arcDir = (hdgErr > 0) ? 1 : -1; + arcOutBearing = legBearing; + phiNomCd = phiTmp; + tEaseMs = tTmp; + // Pin the arc tangent to BOTH legs (corner-cut inscribed circle) so the exit lands ON the + // out-leg, not offset: centre = intersection of the in-leg and out-leg lines, each shifted R + // toward the turn inside. The radial term (STEADY) then converges the aircraft onto it. + const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); + const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); + const float d1x = cos_approx(cogRad), d1y = sin_approx(cogRad); + const float d2x = cos_approx(legRad), d2y = sin_approx(legRad); + const float cross = d1x * d2y - d1y * d2x; + const float p1x = pos->x + arcRtmp * cos_approx(cogRad + arcDir * (M_PIf * 0.5f)); + const float p1y = pos->y + arcRtmp * sin_approx(cogRad + arcDir * (M_PIf * 0.5f)); + if (fabsf(cross) > 0.087f) { // legs not near-parallel (30..160 deg turn) + const float p2x = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(legRad + arcDir * (M_PIf * 0.5f)); + const float p2y = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(legRad + arcDir * (M_PIf * 0.5f)); + const float tt = ((p2x - p1x) * d2y - (p2y - p1y) * d2x) / cross; + arcCx = p1x + tt * d1x; + arcCy = p1y + tt * d1y; + } else { // degenerate -> tangent at the entry point + arcCx = p1x; + arcCy = p1y; + } + } + } + } + if (!active) { + return; + } + } else { + prevLegBearing = legBearing; + } + + rampMs += US2S(deltaMicros) * 1000.0f; + const int32_t hdgErrOut = wrap_18000(arcOutBearing - cog); + + switch (phase) { + case ARC_RAMP_IN: { + const float p = (tEaseMs > 1.0f) ? constrainf(rampMs / tEaseMs, 0.0f, 1.0f) : 1.0f; + const float s = p * p * (3.0f - 2.0f * p); // smoothstep up + fwArcBankCmd = arcDir * phiNomCd * s; + if (p >= 1.0f) { // roll-in done -> track the pre-placed tangent circle + phase = ARC_STEADY; + } + break; + } + case ARC_STEADY: { + const float dx = pos->x - arcCx; + const float dy = pos->y - arcCy; + const float eR = calc_length_pythagorean_2D(dx, dy) - arcR; // [cm], + = outside the arc + const float alpha = atan2_approx(dy, dx); // azimuth on the arc + const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(arcDir * cos_approx(alpha), -arcDir * sin_approx(alpha))))); + const int32_t eH = wrap_18000(tangentBearing - cog); // [centideg] heading error to the arc tangent + fwArcBankCmd = arcDir * (phiNomCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; + if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwArcBankCmd)) { // heading-proportional bank has fallen to the steady bank -> hand the exit to the closed-loop capture + phase = ARC_CAPTURE; + } + break; + } + case ARC_CAPTURE: + default: { + // Closed-loop roll-out: bank proportional to the heading still to go, so bank (and turn rate) reach zero + // exactly as cog reaches the out-leg. Robust to roll-lag / cog-lag / tan nonlinearity -> cannot overshoot. + fwArcBankCmd = constrainf(NAV_FW_ARC_EXIT_GAIN * (float)hdgErrOut, -phiNomCd, phiNomCd); + if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD) { // aligned -> hand back to the standard controller + active = false; + return; + } + break; + } + } + + DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); // bank command [centideg], all phases + DEBUG_SET(DEBUG_FW_TURN, 5, hdgErrOut); // remaining heading to out-leg [centideg]; closed-loop capture -> 0 (should not overshoot) + DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(tEaseMs)); // roll-in ease time [ms] -> sizes the turn-start lead (V*tEase) + fwArcDir = arcDir; + fwArcActive = true; +} + static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t deltaMicros) { if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) { @@ -671,13 +827,21 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { const float turnRadius = getFwCoordinatedTurnRadius(); const float halfAngleTan = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 0.0f, NAV_FW_TURN_LEAD_TAN_MAX); - // velXY term is a ~1 s roll-in lead (replaced by a modelled roll-in in a later PR) - const float turnStartDistance = posControl.actualState.velXY + turnRadius * halfAngleTan; + // Roll-in lead: the smoothstep ramp is back-loaded AND cog (ground track) lags the bank, so the aircraft + // flies nearly straight for longer than the ramp lasts -> the steady arc begins well downrange. Lead by + // k*V*T_in. T_in from roll rate + control_smoothness + control_ease. Coefficient calibrated from flight. + const float easeLeadDistance = posControl.actualState.velXY * (fwTurnEaseTimeMs(getFwPlanningBankDeg()) / 1000.0f) * 1.5f; + float turnStartDistance = easeLeadDistance + turnRadius * halfAngleTan; + // Cap how early the turn may begin (nav_fw_wp_turn_max_lead_time): never more than N ms of flight before the WP. + const float maxLeadDistance = posControl.actualState.velXY * (float)navConfig()->fw.wp_turn_max_lead_time * 0.001f; + const bool turnCapped = turnStartDistance > maxLeadDistance; + turnStartDistance = MIN(turnStartDistance, maxLeadDistance); DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(turnRadius)); DEBUG_SET(DEBUG_FW_TURN, 2, lrintf(turnStartDistance)); DEBUG_SET(DEBUG_FW_TURN, 3, lrintf(posControl.wpDistance)); if (posControl.wpDistance < turnStartDistance) { posControl.flags.wpTurnSmoothingActive = true; + fwFlyByCappedLatch = turnCapped; // capped corner cut -> the arc coordinator flies it direct instead } } @@ -693,6 +857,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY); } + // Arc turn coordinator (Approach B): manages the turn state and commands the roll bank directly + // (applied in updatePositionHeadingController_FW). The position carrot stays on the normal path. + updateFwTurnArc(deltaMicros); + // Calculate virtual waypoint virtualDesiredPosition.x = navGetCurrentActualPositionAndVelocity()->pos.x + posErrorX * (trackingDistance / distanceToActualTarget); virtualDesiredPosition.y = navGetCurrentActualPositionAndVelocity()->pos.y + posErrorY * (trackingDistance / distanceToActualTarget); @@ -779,8 +947,9 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); - /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */ - if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter) { + /* If waypoint tracking enabled force craft toward and closely track along waypoint course line. + * Suppressed while the arc coordinator drives a turn (it tracks the arc, not the straight leg). */ + if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter && !fwArcActive) { if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { @@ -846,8 +1015,12 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta previousTimeMonitoringUpdate = currentTimeUs; } - // Only allow PID integrator to shrink if error is decreasing over time - const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); + // Only allow PID integrator to shrink if error is decreasing over time. + // While the arc coordinator drives the turn the FF sets the bank and the carrot-P does the tracking, + // so freeze the integrator: otherwise it winds up (carrot error keeps one sign) and slams the turn at handback. + const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR + | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0) + | (fwArcActive ? PID_FREEZE_INTEGRATOR : 0); // Input error in (deg*100), output roll angle (deg*100) const float navBankLimit = getFwControlBankLimit(); // planning target on WP turns, guard ceiling in loiter @@ -859,10 +1032,17 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. rollAdjustment += getFwTurnFeedForward(navHeadingError); - // 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; + // Arc turn coordinator drives the roll directly while active (overrides PID+FF). Its smoothstep ramps are + // already gentle, so control_smoothness is bypassed during the arc (CS is folded into the ease time instead); + // the smoother is re-seeded on the first direct frame after an arc handback or a controller reset so a + // stale internal state cannot smear or falsely trigger the S-curve. + if (fwArcActive) { + rollAdjustment = fwArcBankCmd; + fwRollSmoothReseed = true; + } else { + rollAdjustment = applyFwRollInSmoothing(rollAdjustment, deltaMicros, fwRollSmoothReseed); + fwRollSmoothReseed = false; + } rollAdjustment = constrainf(rollAdjustment, -DEGREES_TO_CENTIDEGREES(navBankLimit), DEGREES_TO_CENTIDEGREES(navBankLimit)); // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees) From 677108a767d9c161292152f10a05f5e2a4bd061a Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 20 Aug 2026 15:20:31 +0200 Subject: [PATCH 09/21] FW nav: fix stale arc-coordinator state across nav interruptions Two state-handling fixes in the arc turn coordinator (found in code review): - The coordinator's engage latch and leg-bearing memory were function-local statics that survived resetFixedWingPositionController(). Interrupting nav mid-arc (switch to ANGLE/ALTHOLD/COURSE_HOLD) and re-entering WP mode resumed the arc with stale geometry, banking toward an outdated out-leg. State is now file-scope (fwArcEngaged, fwArcPrevLegBearing) and cleared on controller reset; the roll S-curve smoother is re-seeded the same way (fwRollSmoothReseed) so stale filter state cannot fire a spurious ramp. - A leg change while an arc was still active (short legs: FLY_BY early-reach advances the mission mid-turn) was silently consumed: the arc completed onto the stale out-bearing and the new corner got no coordination at all. Now the closed-loop capture is retargeted onto the new leg (bounded +/-phi_nom, hands back once aligned), so quick consecutive corners degrade gracefully instead of being skipped. --- src/main/navigation/navigation_fixedwing.c | 32 ++++++++++++++-------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 81152a399c9..e7c9b9eb568 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -116,6 +116,8 @@ static timeUs_t fwLastNavRollCmdTimeUs = 0; // nav-to-nav transition apart from static float fwEffectiveBankLimit = 0.0f; // adaptive nav bank limit (energy guard), deg; 0 = not yet initialised static float fwActiveLoiterRadius = 0.0f; // effective loiter radius in use (cm), for the turn feed-forward static bool fwArcActive = false; // arc turn coordinator is driving the turn (-> bank headroom, suppress cross-track, roll override) +static bool fwArcEngaged = false; // arc coordinator latch across loops; must be cleared on controller reset or a stale arc resumes after a nav interruption +static int32_t fwArcPrevLegBearing = -1; // last seen WP leg bearing [centideg] for leg-change detection (-1 = unseeded) static bool fwFlyByCappedLatch = false; // the pending FLY_BY turn hit the lead-time cap -> fly it direct, not as an arc static int8_t fwArcDir = 1; // active arc turn direction (+1 right / -1 left) static float fwArcBankCmd = 0.0f; // direct-radius arc bank command [centideg] (Approach B), applied to roll while fwArcActive @@ -319,6 +321,8 @@ void resetFixedWingPositionController(void) virtualDesiredPosition.y = 0; virtualDesiredPosition.z = 0; fwArcActive = false; + fwArcEngaged = false; + fwArcPrevLegBearing = -1; fwFlyByCappedLatch = false; navPidReset(&posControl.pids.fw_nav); @@ -658,9 +662,7 @@ static float fwTurnEaseTimeMs(float phiNomDeg) static void updateFwTurnArc(timeDelta_t deltaMicros) { enum { ARC_RAMP_IN = 0, ARC_STEADY, ARC_CAPTURE }; - static bool active = false; static uint8_t phase; - static int32_t prevLegBearing = -1; static float arcCx, arcCy, arcR; static int8_t arcDir; static int32_t arcOutBearing; @@ -672,8 +674,8 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const bool wpTracking = isWaypointNavTrackingActive() && !needToCalculateCircularLoiter; if (navConfig()->fw.wp_turn_coordination != NAV_FW_WP_TURN_COORDINATED || !wpTracking) { - active = false; - prevLegBearing = -1; + fwArcEngaged = false; + fwArcPrevLegBearing = -1; return; } @@ -682,9 +684,9 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; const float v = posControl.actualState.velXY; - if (!active) { - const bool legChanged = (prevLegBearing >= 0) && (ABS(wrap_18000(legBearing - prevLegBearing)) > 500); - prevLegBearing = legBearing; + if (!fwArcEngaged) { + const bool legChanged = (fwArcPrevLegBearing >= 0) && (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500); + fwArcPrevLegBearing = legBearing; if (legChanged) { const bool capped = fwFlyByCappedLatch; // a capped FLY_BY turn is flown direct, not as an arc fwFlyByCappedLatch = false; // consume the latch on any leg change @@ -696,7 +698,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / arcRtmp)); // v/R == g*tan(phi)/v const float psiTmp = 0.5f * omegaNomCds * (tTmp / 1000.0f); if (2.0f * psiTmp < (float)ABS(hdgErr)) { // enough turn left for a steady arc between the ease ramps - active = true; + fwArcEngaged = true; phase = ARC_RAMP_IN; rampMs = 0.0f; arcR = arcRtmp; @@ -727,11 +729,19 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } } - if (!active) { + if (!fwArcEngaged) { return; } } else { - prevLegBearing = legBearing; + // Mission advanced mid-arc (short leg): retarget the closed-loop capture onto the new leg instead + // of finishing the turn onto the stale out-bearing. The capture law is bounded (+/- phiNom) and + // hands back once aligned, so consecutive quick corners degrade gracefully instead of being skipped. + if (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500) { + fwFlyByCappedLatch = false; + arcOutBearing = legBearing; + phase = ARC_CAPTURE; + } + fwArcPrevLegBearing = legBearing; } rampMs += US2S(deltaMicros) * 1000.0f; @@ -766,7 +776,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // exactly as cog reaches the out-leg. Robust to roll-lag / cog-lag / tan nonlinearity -> cannot overshoot. fwArcBankCmd = constrainf(NAV_FW_ARC_EXIT_GAIN * (float)hdgErrOut, -phiNomCd, phiNomCd); if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD) { // aligned -> hand back to the standard controller - active = false; + fwArcEngaged = false; return; } break; From fe9d854cdf7c8c6634dd42ae152a762f3754231f Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 20 Aug 2026 15:51:08 +0200 Subject: [PATCH 10/21] FW nav: review follow-ups - lead-time cap floor + energy guard vs manual throttle Maintainer decisions 2026-08-20 after the resume code review: nav_fw_wp_turn_max_lead_time: min 0 -> 1000ms (0 silently disabled the FLY_BY anticipation: lead distance 0 -> WP reached by proximity radius, arc engages uncapped at the corner - an undesigned mode). Stays a permanent user setting instead of being hardcoded before release; DEV note dropped, Settings.md regenerated. Energy bank guard: evaluate the near-throttle-limit branch against the AUTO throttle demand (before allow_manual_thr_increase is added). Pilot-held full throttle permanently armed the branch even though the autopilot still had throttle authority; the throttle branch now cleanly means 'auto-throttle authority exhausted'. A genuine energy crisis is still caught by the OR-connected pitch branch (climb pitch saturates). --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 4 ++-- src/main/navigation/navigation_fixedwing.c | 10 +++++++--- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ea70d02ebeb..472e6335052 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4212,11 +4212,11 @@ DEVELOPER/EXPERIMENTAL (to be hardcoded before release): heading error to the ne ### nav_fw_wp_turn_max_lead_time -DEVELOPER/EXPERIMENTAL (to be hardcoded before release): FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large angles; above this the turn waits, then flies a non-tangent recovery. +FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large turn angles; when the cap limits the turn it starts later and flies a non-tangent recovery onto the next leg. | Default | Min | Max | | --- | --- | --- | -| 3000 | 0 | 10000 | +| 3000 | 1000 | 10000 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c546ab8bc10..a9aa5724c22 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2696,10 +2696,10 @@ groups: min: 5 max: 45 - name: nav_fw_wp_turn_max_lead_time - description: "DEVELOPER/EXPERIMENTAL (to be hardcoded before release): FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large angles; above this the turn waits, then flies a non-tangent recovery." + description: "FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large turn angles; when the cap limits the turn it starts later and flies a non-tangent recovery onto the next leg." default_value: 3000 field: fw.wp_turn_max_lead_time - min: 0 + min: 1000 max: 10000 - name: nav_fw_wp_turn_control_ease description: "DEVELOPER/EXPERIMENTAL: unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramp; SIM low, real models higher." diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e7c9b9eb568..4675b6e499d 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -467,7 +467,7 @@ static float getFwControlBankLimit(void) // Reduce the effective bank limit when a commanded climb can't be sustained near the pitch/throttle // limit while banked, so the turn/loiter widens and the climb recovers. Uses target-vs-actual Vz. -static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t correctedThrottleValue) +static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottleValue) { static timeUs_t lastUpdateUs = 0; static timeUs_t lastTriggerUs = 0; @@ -523,7 +523,10 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t correctedTh const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES((float)navConfig()->fw.max_climb_angle); const bool nearPitchLimit = (float)posControl.rcAdjustment[PITCH] >= NAV_FW_GUARD_PITCH_FRAC * maxClimbDeciDeg; - const bool nearThrottleLimit = correctedThrottleValue >= (currentBatteryProfile->nav.fw.max_throttle - NAV_FW_GUARD_THROTTLE_MARGIN); + // Throttle branch means "auto-throttle authority exhausted": compare the AUTO demand only, so a + // pilot holding manual full throttle (allow_manual_thr_increase) does not permanently arm it. + // A genuine energy crisis is still caught by the pitch branch (climb pitch saturates). + const bool nearThrottleLimit = autoThrottleValue >= (currentBatteryProfile->nav.fw.max_throttle - NAV_FW_GUARD_THROTTLE_MARGIN); const bool climbCommanded = targetVz > NAV_FW_GUARD_VZ_CLIMB_MIN; const bool trigger = banked && climbCommanded && deficitLatched && (nearPitchLimit || nearThrottleLimit); @@ -1212,6 +1215,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat } uint16_t correctedThrottleValue = constrain(cruiseThrottle + throttleCorrection, minThrottle, maxThrottle); + const uint16_t autoThrottleValue = correctedThrottleValue; // auto demand before manual increase, for the energy guard // Manual throttle increase if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { @@ -1228,7 +1232,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false); // Update the energy guard now that this cycle's pitch + throttle commands are known. - updateFwEnergyBankGuard(currentTimeUs, correctedThrottleValue); + updateFwEnergyBankGuard(currentTimeUs, autoThrottleValue); } } From 47e49b332c066d37d014c2c5e98015a3886aef42 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 20 Aug 2026 21:18:24 +0200 Subject: [PATCH 11/21] FW nav: remove leftovers of the superseded arc handback design The arc coordinator's original exit (design v1) handed control back to the heading PID at a fixed heading error (nav_fw_wp_turn_handback_angle, 15 deg) with a 300ms progress guard against premature handback on cog noise (NAV_FW_ARC_HANDBACK_GUARD_MS). Both became obsolete when the exit was replaced by the closed-loop capture phase (bank proportional to remaining heading, cannot hand back early or overshoot) but survived as dead code: the guard define was never referenced, the setting was stored but never read. Remove both (PG_NAV_CONFIG 11 -> 12 for the struct change) and skip the turn feed-forward computation entirely while the arc coordinator drives the roll: its result was discarded (the arc bank command already is the coordinated bank), and its arc branch only fed a debug channel with values that were never applied. fwArcDir is unused after that and removed. --- docs/Settings.md | 10 ---------- src/main/fc/settings.yaml | 6 ------ src/main/navigation/navigation.c | 3 +-- src/main/navigation/navigation.h | 1 - src/main/navigation/navigation_fixedwing.c | 22 ++++++++-------------- 5 files changed, 9 insertions(+), 33 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 472e6335052..def1d83ac31 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4200,16 +4200,6 @@ How FW waypoint turns are flown. COORDINATED (default) commands an explicit coor --- -### nav_fw_wp_turn_handback_angle - -DEVELOPER/EXPERIMENTAL (to be hardcoded before release): heading error to the next leg [deg] at which the arc turn coordinator hands control back to the normal heading PID. - -| Default | Min | Max | -| --- | --- | --- | -| 15 | 5 | 45 | - ---- - ### nav_fw_wp_turn_max_lead_time FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large turn angles; when the cap limits the turn it starts later and flies a non-tangent recovery onto the next leg. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a9aa5724c22..d762f1b05cb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2689,12 +2689,6 @@ groups: default_value: "COORDINATED" field: fw.wp_turn_coordination table: nav_fw_wp_turn_coordination - - name: nav_fw_wp_turn_handback_angle - description: "DEVELOPER/EXPERIMENTAL (to be hardcoded before release): heading error to the next leg [deg] at which the arc turn coordinator hands control back to the normal heading PID." - default_value: 15 - field: fw.wp_turn_handback_angle - min: 5 - max: 45 - name: nav_fw_wp_turn_max_lead_time description: "FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large turn angles; when the cap limits the turn it starts later and flies a non-tangent recovery onto the next leg." default_value: 3000 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 628e165f96e..07d7eba993d 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, 11); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 12); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -253,7 +253,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .wp_turn_mode = SETTING_NAV_FW_WP_TURN_MODE_DEFAULT, // FLY_BY, WP mission turn mode .turn_ff_gain = SETTING_NAV_FW_TURN_FF_GAIN_DEFAULT, // 100, turn FF .wp_turn_coordination = SETTING_NAV_FW_WP_TURN_COORDINATION_DEFAULT, // COORDINATED, arc-based turns - .wp_turn_handback_angle = SETTING_NAV_FW_WP_TURN_HANDBACK_ANGLE_DEFAULT, // 15 deg .wp_turn_max_lead_time = SETTING_NAV_FW_WP_TURN_MAX_LEAD_TIME_DEFAULT, // 3000 ms .wp_turn_control_ease = SETTING_NAV_FW_WP_TURN_CONTROL_EASE_DEFAULT, // 100 ms } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 71ba9d31a61..720f261179c 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -514,7 +514,6 @@ typedef struct navConfig_s { uint8_t wp_turn_mode; // WP mission turn mode (navFwWpTurnMode_e: FLY_BY / FLY_OVER) uint8_t turn_ff_gain; // turn coordination feed-forward gain [%] (0 = off; dev tuning, to be hardcoded) uint8_t wp_turn_coordination; // turn handling (navFwWpTurnCoordination_e: DIRECT / COORDINATED) - uint8_t wp_turn_handback_angle; // arc -> direct PID handback heading error [deg] (dev tuning) uint16_t wp_turn_max_lead_time; // FLY_BY: cap on how early the turn may start before the WP [ms] (dev tuning) uint16_t wp_turn_control_ease; // unmodelled roll-response lag added to the computed turn ease time [ms] } fw; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 4675b6e499d..7aff19ae2a3 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -77,7 +77,6 @@ #define NAV_FW_LOITER_RADIUS_DECAY 100.0f // [cm/s] max rate the held loiter radius eases back down (1 m/s) #define NAV_FW_TURN_LEAD_TAN_MAX 3.7f // tan(half turn angle) cap (~150 deg) to bound the lead distance #define NAV_FW_ARC_MIN_TURN_ANGLE_CD 3000 // [centideg] only fly the coordinated arc for turns sharper than 30 deg -#define NAV_FW_ARC_HANDBACK_GUARD_MS 300.0f // [ms] min time in the arc before handback may fire (anti early-handback) #define NAV_FW_ARC_RADIAL_GAIN 0.5f // [centideg bank / cm radial error] pull back onto the arc radius (TBD from flight) #define NAV_FW_ARC_HEADING_GAIN 0.3f // [centideg bank / centideg tangent heading error] align to the arc (TBD from flight) #define NAV_FW_ARC_EXIT_GAIN 2.0f // [centideg bank / centideg heading error] proportional roll-out capture: bank -> 0 as cog reaches the out-leg (no overshoot) @@ -119,7 +118,6 @@ static bool fwArcActive = false; // arc turn coordinator is driving t static bool fwArcEngaged = false; // arc coordinator latch across loops; must be cleared on controller reset or a stale arc resumes after a nav interruption static int32_t fwArcPrevLegBearing = -1; // last seen WP leg bearing [centideg] for leg-change detection (-1 = unseeded) static bool fwFlyByCappedLatch = false; // the pending FLY_BY turn hit the lead-time cap -> fly it direct, not as an arc -static int8_t fwArcDir = 1; // active arc turn direction (+1 right / -1 left) static float fwArcBankCmd = 0.0f; // direct-radius arc bank command [centideg] (Approach B), applied to roll while fwArcActive static int8_t loiterDirYaw = 1; static bool needToCalculateCircularLoiter; @@ -566,10 +564,7 @@ static float getFwTurnFeedForward(int32_t navHeadingError) float ffRadius = 0.0f; float ffSign = 0.0f; - if (fwArcActive) { // arc turn: full coordinated bank, NO taper (the arc, not the heading error, sets the bank) - ffRadius = getFwCoordinatedTurnRadius(); - ffSign = (float)fwArcDir; - } else if (needToCalculateCircularLoiter) { // loiter circle: known radius + if (needToCalculateCircularLoiter) { // loiter circle: known radius ffRadius = fwActiveLoiterRadius; ffSign = (float)loiterDirection(); } else if (isWaypointNavTrackingActive() && ABS(navHeadingError) > NAV_FW_FF_HEADING_DEADBAND_CD) { @@ -789,7 +784,6 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); // bank command [centideg], all phases DEBUG_SET(DEBUG_FW_TURN, 5, hdgErrOut); // remaining heading to out-leg [centideg]; closed-loop capture -> 0 (should not overshoot) DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(tEaseMs)); // roll-in ease time [ms] -> sizes the turn-start lead (V*tEase) - fwArcDir = arcDir; fwArcActive = true; } @@ -1042,17 +1036,17 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta DEGREES_TO_CENTIDEGREES(navBankLimit), pidFlags); - // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. - rollAdjustment += getFwTurnFeedForward(navHeadingError); - - // Arc turn coordinator drives the roll directly while active (overrides PID+FF). Its smoothstep ramps are - // already gentle, so control_smoothness is bypassed during the arc (CS is folded into the ease time instead); - // the smoother is re-seeded on the first direct frame after an arc handback or a controller reset so a - // stale internal state cannot smear or falsely trigger the S-curve. + // Arc turn coordinator drives the roll directly while active (overrides the PID; FF is skipped, the + // arc bank command already is the coordinated bank). Its smoothstep ramps are already gentle, so + // control_smoothness is bypassed during the arc (CS is folded into the ease time instead); the smoother + // is re-seeded on the first direct frame after an arc handback or a controller reset so a stale + // internal state cannot smear or falsely trigger the S-curve. if (fwArcActive) { rollAdjustment = fwArcBankCmd; fwRollSmoothReseed = true; } else { + // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. + rollAdjustment += getFwTurnFeedForward(navHeadingError); rollAdjustment = applyFwRollInSmoothing(rollAdjustment, deltaMicros, fwRollSmoothReseed); fwRollSmoothReseed = false; } From 120fdb46ea28fb38112969fc792f3dc45f782e8f Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Fri, 21 Aug 2026 15:59:33 +0200 Subject: [PATCH 12/21] FW nav: loiter FF gate, capture fallback for capped/sharp corners, predictive roll-out lead - loiter FF only once established on the circle: fed during the (much larger) approach cone it fought the approach carrot and slewed the entry across the circle - capped and >150 deg corners fly the bounded closed-loop capture instead of the reactive PID / a degenerate tangent circle (flutter) - capture leads the roll-out by omega*(tau + control_ease), tau from the angle-P gain: the airframe sheds bank slower than the command falls, the residual turn rate was overshooting the out-leg - keep the roll smoother's reseed baseline current while the arc drives (a stale reset-time seed caused a brief roll twitch at arc handback) - nav_fw_wp_turn_max_lead_time bounds/default now 3000/6000/12000 (the 3 s ceiling capped nearly every cruise-speed corner) Co-Authored-By: Claude Fable 5 --- docs/Settings.md | 4 +- src/main/fc/settings.yaml | 8 ++-- src/main/navigation/navigation_fixedwing.c | 56 ++++++++++++++++++---- 3 files changed, 53 insertions(+), 15 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index def1d83ac31..b31394987b8 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4202,11 +4202,11 @@ How FW waypoint turns are flown. COORDINATED (default) commands an explicit coor ### nav_fw_wp_turn_max_lead_time -FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large turn angles; when the cap limits the turn it starts later and flies a non-tangent recovery onto the next leg. +FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint. | Default | Min | Max | | --- | --- | --- | -| 3000 | 1000 | 10000 | +| 6000 | 3000 | 12000 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d762f1b05cb..048bd108934 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2690,11 +2690,11 @@ groups: field: fw.wp_turn_coordination table: nav_fw_wp_turn_coordination - name: nav_fw_wp_turn_max_lead_time - description: "FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. Stops extreme early turn-in at high speed / large turn angles; when the cap limits the turn it starts later and flies a non-tangent recovery onto the next leg." - default_value: 3000 + description: "FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint." + default_value: 6000 field: fw.wp_turn_max_lead_time - min: 1000 - max: 10000 + min: 3000 + max: 12000 - name: nav_fw_wp_turn_control_ease description: "DEVELOPER/EXPERIMENTAL: unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramp; SIM low, real models higher." default_value: 100 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 7aff19ae2a3..4065c0f7ba7 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -81,6 +81,7 @@ #define NAV_FW_ARC_HEADING_GAIN 0.3f // [centideg bank / centideg tangent heading error] align to the arc (TBD from flight) #define NAV_FW_ARC_EXIT_GAIN 2.0f // [centideg bank / centideg heading error] proportional roll-out capture: bank -> 0 as cog reaches the out-leg (no overshoot) #define NAV_FW_ARC_EXIT_HANDOFF_CD 150 // [centideg] hand back to the PID within this heading error of the out-leg (keep low: residual bank = gain*this) +#define NAV_FW_ARC_SHARP_TURN_CD 15000 // [centideg] beyond this the tangent points explode toward the 180 deg reversal -> capture-only turn // FW energy/altitude bank guard thresholds (conservative; observable via DEBUG_FW_TURN) #define NAV_FW_GUARD_PHI_FLOOR_DEG 15.0f // minimum effective bank limit @@ -98,6 +99,8 @@ // Turn-coordination feed-forward: heading-error window over which the WP-turn FF tapers in (centideg) #define NAV_FW_FF_HEADING_DEADBAND_CD 500.0f // below this heading error: no WP-turn FF #define NAV_FW_FF_HEADING_FULL_CD 3000.0f // heading error for full WP-turn FF +#define NAV_FW_LOITER_FF_RADIAL_BAND 0.3f // fraction of R: loiter FF only within this band around the circle radius +#define NAV_FW_LOITER_FF_ALIGN_CD 6000 // [centideg] loiter FF only when cog is roughly tangential to the circle // If this is enabled navigation won't be applied if velocity is below 3 m/s //#define NAV_FW_LIMIT_MIN_FLY_VELOCITY @@ -564,9 +567,22 @@ static float getFwTurnFeedForward(int32_t navHeadingError) float ffRadius = 0.0f; float ffSign = 0.0f; - if (needToCalculateCircularLoiter) { // loiter circle: known radius - ffRadius = fwActiveLoiterRadius; - ffSign = (float)loiterDirection(); + if (needToCalculateCircularLoiter) { + // Loiter FF only once established on the circle - fed during the (much larger) approach + // cone it fights the approach carrot and slews the entry across the circle. + const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; + const float dcx = pos->x - posControl.desiredState.pos.x; + const float dcy = pos->y - posControl.desiredState.pos.y; + const float distToCenter = calc_length_pythagorean_2D(dcx, dcy); + const int8_t dir = loiterDirection(); + const float alpha = atan2_approx(dcy, dcx); + const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(dir * cos_approx(alpha), -dir * sin_approx(alpha))))); + const int32_t tangentErr = wrap_18000(tangentBearing - posControl.actualState.cog); + if (fabsf(distToCenter - fwActiveLoiterRadius) < NAV_FW_LOITER_FF_RADIAL_BAND * fwActiveLoiterRadius + && ABS(tangentErr) < NAV_FW_LOITER_FF_ALIGN_CD) { + ffRadius = fwActiveLoiterRadius; + ffSign = (float)dir; + } } else if (isWaypointNavTrackingActive() && ABS(navHeadingError) > NAV_FW_FF_HEADING_DEADBAND_CD) { ffRadius = getFwCoordinatedTurnRadius(); // WP turn: dynamic radius, tapered by heading error ffSign = (navHeadingError > 0 ? 1.0f : -1.0f) * constrainf((float)ABS(navHeadingError) / NAV_FW_FF_HEADING_FULL_CD, 0.0f, 1.0f); @@ -686,16 +702,26 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const bool legChanged = (fwArcPrevLegBearing >= 0) && (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500); fwArcPrevLegBearing = legBearing; if (legChanged) { - const bool capped = fwFlyByCappedLatch; // a capped FLY_BY turn is flown direct, not as an arc + const bool capped = fwFlyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits fwFlyByCappedLatch = false; // consume the latch on any leg change const int32_t hdgErr = wrap_18000(legBearing - cog); - if (!capped && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + if (ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / arcRtmp)); // v/R == g*tan(phi)/v const float psiTmp = 0.5f * omegaNomCds * (tTmp / 1000.0f); - if (2.0f * psiTmp < (float)ABS(hdgErr)) { // enough turn left for a steady arc between the ease ramps + if (capped || ABS(hdgErr) > NAV_FW_ARC_SHARP_TURN_CD) { + // No valid tangent circle (turn started late via the cap, or near-reversal): + // fly the bounded closed-loop capture directly instead of PID / degenerate circle. + fwArcEngaged = true; + phase = ARC_CAPTURE; + rampMs = 0.0f; + arcDir = (hdgErr > 0) ? 1 : -1; + arcOutBearing = legBearing; + phiNomCd = phiTmp; + tEaseMs = tTmp; + } else if (2.0f * psiTmp < (float)ABS(hdgErr)) { // enough turn left for a steady arc between the ease ramps fwArcEngaged = true; phase = ARC_RAMP_IN; rampMs = 0.0f; @@ -770,9 +796,20 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } case ARC_CAPTURE: default: { - // Closed-loop roll-out: bank proportional to the heading still to go, so bank (and turn rate) reach zero - // exactly as cog reaches the out-leg. Robust to roll-lag / cog-lag / tan nonlinearity -> cannot overshoot. - fwArcBankCmd = constrainf(NAV_FW_ARC_EXIT_GAIN * (float)hdgErrOut, -phiNomCd, phiNomCd); + // Closed-loop roll-out, led by the heading the physical roll-out consumes: the angle-P response + // is exponential (tau = 1/(LEVEL_P * multiplier)), so the residual turn integrates to omega*tau, + // plus the unmodelled servo/aero delay (control_ease). Omega from the actual bank and speed. + int32_t captureErr = hdgErrOut; + if (ABS(captureErr) > 17000) { + captureErr = arcDir * ABS(captureErr); // ambiguous reversal: hold the engagement direction + } + const float bankNowRad = CENTIDEGREES_TO_RADIANS((float)ABS(attitude.values.roll) * 10.0f); + const float omegaCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(GRAVITY_CMSS * tan_approx(bankNowRad) / MAX(v, NAV_FW_TURN_MIN_SPEED))); + const float levelGain = pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; // [1/s] + const float rollOutS = ((levelGain > 0.1f) ? (1.0f / levelGain) : 1.0f) + (float)navConfig()->fw.wp_turn_control_ease * 0.001f; + const float psiLeadCd = omegaCds * rollOutS; + const float errLeadCd = MAX((float)ABS(captureErr) - psiLeadCd, 0.0f); + fwArcBankCmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -phiNomCd, phiNomCd); if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD) { // aligned -> hand back to the standard controller fwArcEngaged = false; return; @@ -1043,6 +1080,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // internal state cannot smear or falsely trigger the S-curve. if (fwArcActive) { rollAdjustment = fwArcBankCmd; + fwRollSmoothSeedCd = fwArcBankCmd; // else the handback re-seeds from a stale reset value (brief roll twitch) fwRollSmoothReseed = true; } else { // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. From d64d687fbd1e6c1e46fe2dbce541232ac2069bfe Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Fri, 21 Aug 2026 20:50:29 +0200 Subject: [PATCH 13/21] FW nav: coordinated FLY_OVER (tangent exit) + shaped arc roll-out - FLY_OVER now engages the arc coordinator as a reverse FLY_BY: the turn circle is pinned at the overfly point (led by the roll-in drift) and the exit course is the tangent from that circle through the next waypoint, so the roll-out lands exactly on a straight line to it. Handles any turn angle including full reversals; HITL: exit course within 2 deg of the direct line at all tested corners - shaped roll-out for all arc exits: the capture command's collapse is rate-limited to the entry ramp's build-up rate (phiNom/tEase) with the no-overshoot envelope kept on top; the lead gains the ramp's heading share (0.5*omega*tEase) and STEADY hands over early enough for the ramp to fit; handback waits until nearly level - debug: ch2 = active exit course while the arc runs Co-Authored-By: Claude Fable 5 --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_fixedwing.c | 81 ++++++++++++++++++---- 3 files changed, 71 insertions(+), 14 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index b31394987b8..e31fbd15196 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4212,7 +4212,7 @@ FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. The req ### nav_fw_wp_turn_mode -How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then turns onto the next leg. +How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint. | Allowed Values | | | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 048bd108934..4d436ded18f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2674,7 +2674,7 @@ groups: min: 30 max: 80 - name: nav_fw_wp_turn_mode - description: "How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then turns onto the next leg." + description: "How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint." default_value: "FLY_BY" field: fw.wp_turn_mode table: nav_fw_wp_turn_mode diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 4065c0f7ba7..dcc8818297b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -704,8 +704,51 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (legChanged) { const bool capped = fwFlyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits fwFlyByCappedLatch = false; // consume the latch on any leg change + if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_OVER) { + // FLY_OVER: reverse FLY_BY - the circle is pinned at the overfly point (tangent to the + // current course) and the exit course is the tangent from that circle through the next + // WP, so the roll-out lands exactly on a straight line to it (circle-straight intercept). + const float px = posControl.activeWaypoint.pos.x; + const float py = posControl.activeWaypoint.pos.y; + const int32_t brgToWp = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(py - pos->y, px - pos->x))))); + const int32_t toWpErr = wrap_18000(brgToWp - cog); + if (ABS(toWpErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + const float arcRtmp = getFwCoordinatedTurnRadius(); + const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); + const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); + const int8_t dirTmp = (toWpErr > 0) ? 1 : -1; + const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); + // Pin the circle ahead by the roll-in drift (the FLY_BY easeLead), so the ramp + // ends ON the circle instead of leaving a radial error that slams the bank. + const float leadDist = 1.5f * v * (tTmp / 1000.0f); + const float cx = pos->x + leadDist * cos_approx(cogRad) + arcRtmp * cos_approx(cogRad + dirTmp * (M_PIf * 0.5f)); + const float cy = pos->y + leadDist * sin_approx(cogRad) + arcRtmp * sin_approx(cogRad + dirTmp * (M_PIf * 0.5f)); + const float dCP = calc_length_pythagorean_2D(px - cx, py - cy); + if (dCP > 1.05f * arcRtmp) { // next WP outside the circle: a tangent exists + const float alphaCP = atan2_approx(py - cy, px - cx); + const float phiT = acos_approx(constrainf(arcRtmp / dCP, 0.0f, 1.0f)); + for (int8_t s = -1; s <= 1; s += 2) { // of the two tangent points, exit where the tangent points at the WP + const float th = alphaCP + (float)s * phiT; + const float tx = cx + arcRtmp * cos_approx(th); + const float ty = cy + arcRtmp * sin_approx(th); + if ((px - tx) * (-dirTmp * sin_approx(th)) + (py - ty) * (dirTmp * cos_approx(th)) > 0.0f) { + fwArcEngaged = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + arcR = arcRtmp; + arcDir = dirTmp; + arcCx = cx; + arcCy = cy; + arcOutBearing = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(py - ty, px - tx))))); + phiNomCd = phiTmp; + tEaseMs = tTmp; + } + } + } + } + } const int32_t hdgErr = wrap_18000(legBearing - cog); - if (ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_OVER && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); @@ -716,6 +759,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) // fly the bounded closed-loop capture directly instead of PID / degenerate circle. fwArcEngaged = true; phase = ARC_CAPTURE; + fwArcBankCmd = 0.0f; // fresh engagement: don't rate-limit against a stale command rampMs = 0.0f; arcDir = (hdgErr > 0) ? 1 : -1; arcOutBearing = legBearing; @@ -771,6 +815,15 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) rampMs += US2S(deltaMicros) * 1000.0f; const int32_t hdgErrOut = wrap_18000(arcOutBearing - cog); + // Roll-out prediction, shared by STEADY (early capture hand-over) and CAPTURE (lead): heading + // consumed by the shaped down-ramp (0.5*omega*tEase, smoothstep integral) plus the angle-P + // response tail (tau = 1/(LEVEL_P * multiplier)) and the unmodelled servo/aero delay. + const float bankNowRad = CENTIDEGREES_TO_RADIANS((float)ABS(attitude.values.roll) * 10.0f); + const float omegaCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(GRAVITY_CMSS * tan_approx(bankNowRad) / MAX(v, NAV_FW_TURN_MIN_SPEED))); + const float levelGain = pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; // [1/s] + const float rollOutS = ((levelGain > 0.1f) ? (1.0f / levelGain) : 1.0f) + (float)navConfig()->fw.wp_turn_control_ease * 0.001f; + const float psiLeadCd = omegaCds * rollOutS + 0.5f * omegaCds * (tEaseMs / 1000.0f); + switch (phase) { case ARC_RAMP_IN: { const float p = (tEaseMs > 1.0f) ? constrainf(rampMs / tEaseMs, 0.0f, 1.0f) : 1.0f; @@ -789,28 +842,31 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(arcDir * cos_approx(alpha), -arcDir * sin_approx(alpha))))); const int32_t eH = wrap_18000(tangentBearing - cog); // [centideg] heading error to the arc tangent fwArcBankCmd = arcDir * (phiNomCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; - if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwArcBankCmd)) { // heading-proportional bank has fallen to the steady bank -> hand the exit to the closed-loop capture + if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwArcBankCmd) + || (float)ABS(hdgErrOut) <= psiLeadCd) { // remaining heading fits the shaped roll-out -> start it phase = ARC_CAPTURE; } break; } case ARC_CAPTURE: default: { - // Closed-loop roll-out, led by the heading the physical roll-out consumes: the angle-P response - // is exponential (tau = 1/(LEVEL_P * multiplier)), so the residual turn integrates to omega*tau, - // plus the unmodelled servo/aero delay (control_ease). Omega from the actual bank and speed. + // Closed-loop roll-out: the no-overshoot envelope (bank proportional to the led remaining + // heading), with the command's collapse rate-limited to the entry ramp's build-up rate so + // the level-off is eased instead of an angle-P slam. Magnitude growth stays unrestricted. int32_t captureErr = hdgErrOut; if (ABS(captureErr) > 17000) { captureErr = arcDir * ABS(captureErr); // ambiguous reversal: hold the engagement direction } - const float bankNowRad = CENTIDEGREES_TO_RADIANS((float)ABS(attitude.values.roll) * 10.0f); - const float omegaCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(GRAVITY_CMSS * tan_approx(bankNowRad) / MAX(v, NAV_FW_TURN_MIN_SPEED))); - const float levelGain = pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; // [1/s] - const float rollOutS = ((levelGain > 0.1f) ? (1.0f / levelGain) : 1.0f) + (float)navConfig()->fw.wp_turn_control_ease * 0.001f; - const float psiLeadCd = omegaCds * rollOutS; const float errLeadCd = MAX((float)ABS(captureErr) - psiLeadCd, 0.0f); - fwArcBankCmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -phiNomCd, phiNomCd); - if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD) { // aligned -> hand back to the standard controller + float cmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -phiNomCd, phiNomCd); + const float maxStepCd = phiNomCd * (US2S(deltaMicros) * 1000.0f) / MAX(tEaseMs, 1.0f); + if (fwArcBankCmd > 0.0f) { + cmd = MAX(cmd, fwArcBankCmd - maxStepCd); + } else if (fwArcBankCmd < 0.0f) { + cmd = MIN(cmd, fwArcBankCmd + maxStepCd); + } + fwArcBankCmd = cmd; + if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwArcBankCmd) <= phiNomCd * 0.1f) { // aligned and nearly level -> hand back fwArcEngaged = false; return; } @@ -818,6 +874,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } + DEBUG_SET(DEBUG_FW_TURN, 2, arcOutBearing); // exit course while the arc is active; FLY_OVER: tangent through the next WP DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); // bank command [centideg], all phases DEBUG_SET(DEBUG_FW_TURN, 5, hdgErrOut); // remaining heading to out-leg [centideg]; closed-loop capture -> 0 (should not overshoot) DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(tEaseMs)); // roll-in ease time [ms] -> sizes the turn-start lead (V*tEase) From dca00944aeb216f5be19f3eba756dae61cec7264 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Fri, 21 Aug 2026 22:24:47 +0200 Subject: [PATCH 14/21] FW nav: coordinated FLY_INTO - aligned WP crossing via internal-tangent S Third wp_turn_mode: turn away onto a counter arc first, then cross the WP already aligned on the outbound course. The two equal-radius circles are spaced sqrt((2R)^2 + Ls^2) so an internal-tangent gap (Ls = 2 ease times of travel) gives the roll reversal room; the away arc rolls out onto the tangent course through the shared capture predictor and the main arc is picked up at the touch point with a standard ramp. Scales itself with the corner angle and degenerates cleanly to a teardrop at a full 180 reversal - no fallback needed. ARC_STEADY feed-forward now tracks current groundspeed (phi = atan(v^2/gR)) so wind-driven speed change along the arc is commanded immediately instead of recovered through the radial error term. Mission legs only (NAV_AUTO_WP) - the landing approach keeps FLY_BY. Debug ch1 shows the FLY_INTO sequencer stage (energy-guard bank-limit write muted in that mode). PG_NAV_CONFIG 12 -> 13: the restack onto the updated fw-roll-smoothing head pulled in the cruise_lock_on_level field, changing the struct layout. Co-Authored-By: Claude Fable 5 --- docs/Settings.md | 3 +- src/main/fc/settings.yaml | 4 +- src/main/navigation/navigation.c | 6 +- src/main/navigation/navigation.h | 3 +- src/main/navigation/navigation_fixedwing.c | 127 ++++++++++++++++++++- 5 files changed, 132 insertions(+), 11 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index e31fbd15196..9f6834ee23e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4212,12 +4212,13 @@ FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. The req ### nav_fw_wp_turn_mode -How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint. +How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint. FLY_INTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries). | Allowed Values | | | --- | --- | | FLY_BY | Default | | FLY_OVER | | +| FLY_INTO | | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4d436ded18f..456887563aa 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -181,7 +181,7 @@ tables: values: ["2D", "3D"] enum: dynamicGyroNotchMode_e - name: nav_fw_wp_turn_mode - values: ["FLY_BY", "FLY_OVER"] + values: ["FLY_BY", "FLY_OVER", "FLY_INTO"] enum: navFwWpTurnMode_e - name: nav_fw_wp_turn_coordination values: ["DIRECT", "COORDINATED"] @@ -2674,7 +2674,7 @@ groups: min: 30 max: 80 - name: nav_fw_wp_turn_mode - description: "How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint." + description: "How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint. FLY_INTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries)." default_value: "FLY_BY" field: fw.wp_turn_mode table: nav_fw_wp_turn_mode diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 07d7eba993d..1d2ac8f64b6 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, 12); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 13); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -4304,8 +4304,8 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint) mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3)); calculateAndSetActiveWaypointToLocalPosition(&localPos); - // Turn anticipation (nextTurnAngle) is only needed for FLY_BY; FLY_OVER flies to the WP then turns. - if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_BY) { + // Turn anticipation (nextTurnAngle) is needed for FLY_BY and FLY_INTO; FLY_OVER flies to the WP then turns. + if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_OVER) { fpVector3_t posNextWp; if (getLocalPosNextWaypoint(&posNextWp)) { int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 720f261179c..a9f65f01ea4 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -336,7 +336,8 @@ typedef enum { typedef enum { NAV_FW_WP_TURN_MODE_FLY_BY = 0, // corner cut: turn anticipated so the arc joins the next leg, WP passed abeam - NAV_FW_WP_TURN_MODE_FLY_OVER = 1, // fly over the WP, then turn onto the next leg + NAV_FW_WP_TURN_MODE_FLY_OVER = 1, // fly over the WP, then roll out on the tangent line to the next WP + NAV_FW_WP_TURN_MODE_FLY_INTO = 2, // ease away before the WP, then cross it already aligned on the outbound course } navFwWpTurnMode_e; typedef enum { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index dcc8818297b..66bed3159ce 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -544,7 +544,9 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl } fwEffectiveBankLimit = constrainf(fwEffectiveBankLimit, NAV_FW_GUARD_PHI_FLOOR_DEG, maxBank); - DEBUG_SET(DEBUG_FW_TURN, 1, lrintf(fwEffectiveBankLimit)); + if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_INTO) { // ch1 owned by the FLY_INTO stage diagnostic in that mode + DEBUG_SET(DEBUG_FW_TURN, 1, lrintf(fwEffectiveBankLimit)); + } // ch4/5/6 temporarily owned by the arc coordinator diagnostic (guard deficit/rise/trigger muted) } @@ -683,6 +685,18 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) static float phiNomCd; // coordinated nominal bank for this turn [centideg] static float tEaseMs; // roll-in ease time static float rampMs; // elapsed time in the ramp-in phase + static float rampStartCd; // bank the ramp blends from (0 on entry; -phi at the FLY_INTO inflection) + + // FLY_INTO sequencer: counter-arc away from the corner (AWAY), inflection hand-over, then the + // main arc that crosses the WP already aligned on the outbound course (MAIN). The two circles + // touch (|O1-O2| = 2R), so the S scales itself with the turn angle - no straight in between. + enum { FW_INTO_IDLE = 0, FW_INTO_AWAY, FW_INTO_MAIN, FW_INTO_DONE }; + static uint8_t intoStage; + static float intoEx, intoEy; // inflection point (midpoint of the two centres) + static float intoO2x, intoO2y; // main-arc centre (pinned to the WP + outbound course) + static float intoR; + static int32_t intoBOut; + static int8_t intoDir; fwArcActive = false; @@ -690,6 +704,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (navConfig()->fw.wp_turn_coordination != NAV_FW_WP_TURN_COORDINATED || !wpTracking) { fwArcEngaged = false; fwArcPrevLegBearing = -1; + intoStage = FW_INTO_IDLE; return; } @@ -698,10 +713,15 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; const float v = posControl.actualState.velXY; + if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_INTO) { + DEBUG_SET(DEBUG_FW_TURN, 1, intoStage); // sequencer stage (FLY_INTO diagnostics) + } + if (!fwArcEngaged) { const bool legChanged = (fwArcPrevLegBearing >= 0) && (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500); fwArcPrevLegBearing = legBearing; if (legChanged) { + intoStage = FW_INTO_IDLE; // a new leg invalidates any staged FLY_INTO geometry const bool capped = fwFlyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits fwFlyByCappedLatch = false; // consume the latch on any leg change if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_OVER) { @@ -735,6 +755,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) fwArcEngaged = true; phase = ARC_RAMP_IN; rampMs = 0.0f; + rampStartCd = 0.0f; arcR = arcRtmp; arcDir = dirTmp; arcCx = cx; @@ -761,6 +782,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) phase = ARC_CAPTURE; fwArcBankCmd = 0.0f; // fresh engagement: don't rate-limit against a stale command rampMs = 0.0f; + rampStartCd = 0.0f; arcDir = (hdgErr > 0) ? 1 : -1; arcOutBearing = legBearing; phiNomCd = phiTmp; @@ -769,6 +791,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) fwArcEngaged = true; phase = ARC_RAMP_IN; rampMs = 0.0f; + rampStartCd = 0.0f; arcR = arcRtmp; arcDir = (hdgErr > 0) ? 1 : -1; arcOutBearing = legBearing; @@ -797,6 +820,84 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } } + if (!fwArcEngaged && navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_INTO && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { + if (intoStage == FW_INTO_MAIN) { + // crossing flown: re-arm once the leg has switched (normally it already has, mid-arc) + intoStage = (ABS(wrap_18000(legBearing - intoBOut)) < 500) ? FW_INTO_IDLE : FW_INTO_DONE; + } + if (intoStage == FW_INTO_IDLE) { + const int32_t nta = posControl.activeWaypoint.nextTurnAngle; + if (nta != -1 && ABS(nta) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + const float arcRtmp = getFwCoordinatedTurnRadius(); + const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); + const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); + const int8_t dirM = (nta > 0) ? 1 : -1; + const int32_t bOut = wrap_36000(legBearing + nta); + const float bOutRad = CENTIDEGREES_TO_RADIANS((float)bOut); + const float bInRad = CENTIDEGREES_TO_RADIANS((float)legBearing); + const float ux = cos_approx(bInRad), uy = sin_approx(bInRad); + // Main circle pinned to the WP + outbound course; counter circle tangent to the + // inbound leg on the opposite side. Centers spaced sqrt((2R)^2 + Ls^2): an internal- + // tangent gap Ls stays between the arcs as room for the roll swing that touching + // circles would demand instantaneously. + const float Ls = 2.0f * v * (tTmp / 1000.0f); + const float o2x = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(bOutRad + dirM * (M_PIf * 0.5f)); + const float o2y = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(bOutRad + dirM * (M_PIf * 0.5f)); + const float ax = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(bInRad - dirM * (M_PIf * 0.5f)); + const float ay = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(bInRad - dirM * (M_PIf * 0.5f)); + const float wx = o2x - ax, wy = o2y - ay; + const float wu = wx * ux + wy * uy; + const float disc = wu * wu - (wx * wx + wy * wy) + 4.0f * arcRtmp * arcRtmp + Ls * Ls; + if (disc > 0.0f) { + const float s = wu - sqrtf(disc); // signed along-leg offset of the S start from the WP + const float triggerDist = -s + 1.5f * v * (tTmp / 1000.0f); + if (s < 0.0f && posControl.wpDistance < triggerDist) { + const float o1x = ax + s * ux; + const float o1y = ay + s * uy; + const float cAng = atan2_approx(o2y - o1y, o2x - o1x); + const float beta = atan2_approx(Ls, 2.0f * arcRtmp); + const float nAng = cAng + (float)dirM * beta; + intoEx = o2x - arcRtmp * cos_approx(nAng); // main-arc pickup = internal-tangent touch on the main circle + intoEy = o2y - arcRtmp * sin_approx(nAng); + intoO2x = o2x; intoO2y = o2y; + intoR = arcRtmp; intoBOut = bOut; intoDir = dirM; + fwArcEngaged = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = 0.0f; + arcR = arcRtmp; + arcDir = -dirM; // counter-arc first + arcCx = o1x; + arcCy = o1y; + arcOutBearing = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES( + cAng - (float)dirM * (M_PIf * 0.5f - beta))))); // internal-tangent course: the away arc rolls out onto it + phiNomCd = phiTmp; + tEaseMs = tTmp; + intoStage = FW_INTO_AWAY; + } + } + } + } else if (intoStage == FW_INTO_AWAY) { + // away-arc handed back early (aligned at the inflection course): pick up the main arc + const float distI = calc_length_pythagorean_2D(intoEx - pos->x, intoEy - pos->y); + const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * intoR))); + const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); + if (distI <= 1.5f * v * (tTmp / 1000.0f)) { + fwArcEngaged = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = 0.0f; + arcR = intoR; + arcDir = intoDir; + arcCx = intoO2x; + arcCy = intoO2y; + arcOutBearing = intoBOut; + phiNomCd = phiTmp; + tEaseMs = tTmp; + intoStage = FW_INTO_MAIN; + } + } + } if (!fwArcEngaged) { return; } @@ -810,6 +911,20 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) phase = ARC_CAPTURE; } fwArcPrevLegBearing = legBearing; + + // FLY_INTO fallback: away arc still engaged at the pickup point (capture has not handed back) - swing over directly + if (intoStage == FW_INTO_AWAY + && calc_length_pythagorean_2D(intoEx - pos->x, intoEy - pos->y) <= 1.5f * v * (tEaseMs / 1000.0f)) { + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = fwArcBankCmd; // swing -phi -> +phi through the inflection + arcR = intoR; + arcDir = intoDir; + arcCx = intoO2x; + arcCy = intoO2y; + arcOutBearing = intoBOut; + intoStage = FW_INTO_MAIN; + } } rampMs += US2S(deltaMicros) * 1000.0f; @@ -828,7 +943,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) case ARC_RAMP_IN: { const float p = (tEaseMs > 1.0f) ? constrainf(rampMs / tEaseMs, 0.0f, 1.0f) : 1.0f; const float s = p * p * (3.0f - 2.0f * p); // smoothstep up - fwArcBankCmd = arcDir * phiNomCd * s; + fwArcBankCmd = rampStartCd + ((float)arcDir * phiNomCd - rampStartCd) * s; if (p >= 1.0f) { // roll-in done -> track the pre-placed tangent circle phase = ARC_STEADY; } @@ -841,7 +956,10 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float alpha = atan2_approx(dy, dx); // azimuth on the arc const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(arcDir * cos_approx(alpha), -arcDir * sin_approx(alpha))))); const int32_t eH = wrap_18000(tangentBearing - cog); // [centideg] heading error to the arc tangent - fwArcBankCmd = arcDir * (phiNomCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; + // Feed-forward from CURRENT groundspeed: wind changes v along the arc, so the frozen + // engagement bank would leave the radial feedback carrying the whole v^2 shift. + const float phiLiveCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcR))); + fwArcBankCmd = arcDir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwArcBankCmd) || (float)ABS(hdgErrOut) <= psiLeadCd) { // remaining heading fits the shaped roll-out -> start it phase = ARC_CAPTURE; @@ -925,7 +1043,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t * at any speed. Only runs when nextTurnAngle is set (FLY_BY waypoints + landing); FLY_OVER skips it. */ int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); posControl.flags.wpTurnSmoothingActive = false; - if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { + const bool flyIntoMissionLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_INTO && (navGetCurrentStateFlags() & NAV_AUTO_WP); + if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && !flyIntoMissionLeg && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { const float turnRadius = getFwCoordinatedTurnRadius(); const float halfAngleTan = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 0.0f, NAV_FW_TURN_LEAD_TAN_MAX); // Roll-in lead: the smoothstep ramp is back-loaded AND cog (ground track) lags the bank, so the aircraft From 8cb4b7557298b50cf1dab19cdb4e0f27e96a5a2b Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Sat, 22 Aug 2026 13:10:07 +0200 Subject: [PATCH 15/21] FW nav: coordinated FLY_OVER with path tracking - bounded-intercept S onto the leg With nav_fw_wp_tracking_accuracy enabled, FLY_OVER now rolls out ON the new leg instead of on the direct course to the next WP: the main arc exits onto a bounded intercept course (gamma = half the turn, capped at 45 deg), a short straight gives the bank reversal room, and a standard corner-cut arc rolls out tangentially on the line. At the pickup the second circle is re-solved from CURRENT groundspeed (the arming radius may be unflyable downwind) and anchored along the leg line through the actual position, so wind drift becomes an along-track shift instead of a parallel roll-out offset. Robustness from the same HITL campaign: the arc bank command is clamped to the effective ceiling (wind can drive the radial term arbitrarily large, and rate limits, handoff checks and the smoother seed must not run on a command the airframe cannot reach); no hand-back mid-S (path tracking must not see the transient offset); ramp duration scales with the commanded bank span; the cross-track rate estimator is seeded from the geometric closing speed on hand-back so path tracking re-engages without commanding a full-error kick; comments trimmed to project style. HITL (X-Plane, 15 km/h wind): all corners incl. the 180 reversal and the sharpest downwind corner roll out within 3-9 m of the line, hand-back course within 2 deg; wind-free within 0.5-6.5 m. Co-Authored-By: Claude Fable 5 --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_fixedwing.c | 237 +++++++++++++-------- 3 files changed, 145 insertions(+), 96 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 9f6834ee23e..6fec18a10a6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4212,7 +4212,7 @@ FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. The req ### nav_fw_wp_turn_mode -How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint. FLY_INTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries). +How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint - or, with nav_fw_wp_tracking_accuracy enabled, an S-turn that rolls out directly ON the new leg. FLY_INTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries). | Allowed Values | | | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 456887563aa..f3473117ae3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2674,7 +2674,7 @@ groups: min: 30 max: 80 - name: nav_fw_wp_turn_mode - description: "How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint. FLY_INTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries)." + description: "How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint - or, with nav_fw_wp_tracking_accuracy enabled, an S-turn that rolls out directly ON the new leg. FLY_INTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries)." default_value: "FLY_BY" field: fw.wp_turn_mode table: nav_fw_wp_turn_mode diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 66bed3159ce..8167ed31d33 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -435,39 +435,32 @@ static float applyFwRollInSmoothing(float rollTargetCd, timeDelta_t deltaMicros, return out; } -// Hard roll ceiling [deg] = the global angle-mode limit (max_angle_inclination_rll); the nav control -// output may never exceed it (also enforced downstream by pidAngleToRcCommand). +// Hard roll ceiling [deg]: the global angle-mode limit (max_angle_inclination_rll) static float getFwBankCeilingDeg(void) { return (float)pidProfile()->max_angle_inclination[FD_ROLL] / 10.0f; } -// Control-output bank ceiling [deg]: the hard roll ceiling, reduced by the energy guard. Roll PID/FF -// corrections may climb to here to HOLD the radius; nav_fw_bank_angle is only the planning target. +// Output bank ceiling [deg]: hard ceiling reduced by the energy guard; nav_fw_bank_angle is only the planning target static float getFwEffectiveBankLimit(void) { const float ceiling = getFwBankCeilingDeg(); return (fwEffectiveBankLimit > 0.0f) ? MIN(fwEffectiveBankLimit, ceiling) : ceiling; } -// Planning bank [deg] for sizing turn/loiter radii: nav_fw_bank_angle as the TARGET, capped by the -// (guard-reduced) ceiling. If nav_fw_bank_angle >= the ceiling, planning == ceiling (hard limit). +// Planning bank [deg] for sizing turn/loiter radii: the target, capped by the guard ceiling static float getFwPlanningBankDeg(void) { return MIN((float)navConfig()->fw.max_bank_angle, getFwEffectiveBankLimit()); } -// Roll-command bank limit [deg]: a held loiter OR an active arc may use the reserve up to the guard -// ceiling — the loiter to hold its circle, the arc's radial term to pull back onto the radius against -// wind. (Approach B commands the bank directly with no lead bias, so the reserve is used only for -// genuine radial error, never over-banked.) Everywhere else (direct/capped/shallow turns, cruise) = target. +// Roll-command bank limit [deg]: held loiter / active arc may use the reserve up to the ceiling to hold the radius against wind; everywhere else the target static float getFwControlBankLimit(void) { return ((navGetCurrentStateFlags() & NAV_CTL_HOLD) || fwArcActive) ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); } -// Reduce the effective bank limit when a commanded climb can't be sustained near the pitch/throttle -// limit while banked, so the turn/loiter widens and the climb recovers. Uses target-vs-actual Vz. +// Reduce the bank ceiling when a commanded climb stalls near the pitch/throttle limit while banked, so the turn widens and the climb recovers static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottleValue) { static timeUs_t lastUpdateUs = 0; @@ -524,9 +517,7 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES((float)navConfig()->fw.max_climb_angle); const bool nearPitchLimit = (float)posControl.rcAdjustment[PITCH] >= NAV_FW_GUARD_PITCH_FRAC * maxClimbDeciDeg; - // Throttle branch means "auto-throttle authority exhausted": compare the AUTO demand only, so a - // pilot holding manual full throttle (allow_manual_thr_increase) does not permanently arm it. - // A genuine energy crisis is still caught by the pitch branch (climb pitch saturates). + // AUTO throttle demand only: manual full throttle must not permanently arm the guard const bool nearThrottleLimit = autoThrottleValue >= (currentBatteryProfile->nav.fw.max_throttle - NAV_FW_GUARD_THROTTLE_MARGIN); const bool climbCommanded = targetVz > NAV_FW_GUARD_VZ_CLIMB_MIN; @@ -544,7 +535,7 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl } fwEffectiveBankLimit = constrainf(fwEffectiveBankLimit, NAV_FW_GUARD_PHI_FLOOR_DEG, maxBank); - if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_INTO) { // ch1 owned by the FLY_INTO stage diagnostic in that mode + if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_BY) { // ch1 owned by the S-sequencer stage diagnostic otherwise DEBUG_SET(DEBUG_FW_TURN, 1, lrintf(fwEffectiveBankLimit)); } // ch4/5/6 temporarily owned by the arc coordinator diagnostic (guard deficit/rise/trigger muted) @@ -570,8 +561,7 @@ static float getFwTurnFeedForward(int32_t navHeadingError) float ffRadius = 0.0f; float ffSign = 0.0f; if (needToCalculateCircularLoiter) { - // Loiter FF only once established on the circle - fed during the (much larger) approach - // cone it fights the approach carrot and slews the entry across the circle. + // FF only once established on the circle - during the approach it fights the approach guidance const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; const float dcx = pos->x - posControl.desiredState.pos.x; const float dcy = pos->y - posControl.desiredState.pos.y; @@ -600,11 +590,8 @@ static float getFwTurnFeedForward(int32_t navHeadingError) return rollFF; } -// Loiter-radius floor [cm], stabilised. The tightest holdable circle scales with ground-speed squared, -// so it swings with wind; commanding that every loop makes the fixed-radius loiter tracker thrash. We -// ratchet UP immediately (safety), hold the PEAK over a full revolution, then ease DOWN toward that -// revolution's peak at <= NAV_FW_LOITER_RADIUS_DECAY (no abrupt drop after a gust). Revolution = the -// aircraft's azimuth about the loiter centre sweeping a net 360deg (heading-independent). +// Stabilised loiter-radius floor [cm]: the raw requirement swings with wind (v^2) and would make the +// tracker thrash - ratchet up instantly, hold the peak one revolution, ease down at <= DECAY static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearingFromCenterRad, bool loiterActive, timeDelta_t deltaMicros) { static bool active = false; @@ -658,10 +645,8 @@ static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearing return out; } -// Modelled roll-in/out ease time [ms]: how long to ramp the bank to phiNom (and back). Roll-rate floor -// (1.5x so a smoothstep ramp's peak rate == roll_rate), the control_smoothness window folded in (gentleness -// / structural protection), plus the user's unmodelled servo+inertia margin. CS is bypassed during the arc, -// so this is the single, deterministic source of the turn's roll dynamics. +// Roll-in/out ease time [ms] from roll rate, control_smoothness and the servo/inertia margin - +// single source of the turn's roll dynamics (the S-curve smoother is bypassed during the arc) static float fwTurnEaseTimeMs(float phiNomDeg) { const float rollRateDps = currentControlProfile->stabilized.rates[FD_ROLL] * 10.0f; @@ -670,11 +655,8 @@ static float fwTurnEaseTimeMs(float phiNomDeg) return rollMs + csMs + (float)navConfig()->fw.wp_turn_control_ease; } -// Arc-based turn coordinator (Approach B + roll-aware easing): on a real WP-to-WP turn (course change > 30 deg) -// fly a variable-radius spline = smoothstep bank ramp 0->phiNom (RAMP_IN) -> coordinated arc (STEADY, direct -// radius control: nominal + radial pull-back kR + tangent alignment kH) -> ramp phiNom->0 (RAMP_OUT) -> hand -// back to the PID level + aligned. The ramps make the command achievable (no slam, no roll-in drift); the -// entry is anticipated geometrically by the FLY_BY turnStartDistance. Sets fwArcActive (drives roll directly). +// Arc turn coordinator: bank ramp -> coordinated arc (radius + tangent feedback) -> predictive +// capture roll-out. Sets fwArcActive (drives the roll directly). static void updateFwTurnArc(timeDelta_t deltaMicros) { enum { ARC_RAMP_IN = 0, ARC_STEADY, ARC_CAPTURE }; @@ -687,13 +669,11 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) static float rampMs; // elapsed time in the ramp-in phase static float rampStartCd; // bank the ramp blends from (0 on entry; -phi at the FLY_INTO inflection) - // FLY_INTO sequencer: counter-arc away from the corner (AWAY), inflection hand-over, then the - // main arc that crosses the WP already aligned on the outbound course (MAIN). The two circles - // touch (|O1-O2| = 2R), so the S scales itself with the turn angle - no straight in between. + // S sequencer (FLY_INTO / FLY_OVER-tracking): first arc, internal-tangent gap, second arc enum { FW_INTO_IDLE = 0, FW_INTO_AWAY, FW_INTO_MAIN, FW_INTO_DONE }; static uint8_t intoStage; - static float intoEx, intoEy; // inflection point (midpoint of the two centres) - static float intoO2x, intoO2y; // main-arc centre (pinned to the WP + outbound course) + static float intoEx, intoEy; // second-arc pickup point (internal-tangent touch) + static float intoO2x, intoO2y; // second-arc centre static float intoR; static int32_t intoBOut; static int8_t intoDir; @@ -713,21 +693,20 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; const float v = posControl.actualState.velXY; - if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_INTO) { - DEBUG_SET(DEBUG_FW_TURN, 1, intoStage); // sequencer stage (FLY_INTO diagnostics) + if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_BY) { + DEBUG_SET(DEBUG_FW_TURN, 1, intoStage); // S-sequencer stage (FLY_INTO / FLY_OVER-tracking diagnostics) } if (!fwArcEngaged) { const bool legChanged = (fwArcPrevLegBearing >= 0) && (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500); fwArcPrevLegBearing = legBearing; if (legChanged) { - intoStage = FW_INTO_IDLE; // a new leg invalidates any staged FLY_INTO geometry + intoStage = FW_INTO_IDLE; // a new leg invalidates any staged S geometry const bool capped = fwFlyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits fwFlyByCappedLatch = false; // consume the latch on any leg change if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_OVER) { - // FLY_OVER: reverse FLY_BY - the circle is pinned at the overfly point (tangent to the - // current course) and the exit course is the tangent from that circle through the next - // WP, so the roll-out lands exactly on a straight line to it (circle-straight intercept). + // FLY_OVER: circle pinned at the overfly point. Tracking OFF: exit on the tangent + // through the next WP; tracking ON: bounded-intercept S onto the new leg itself. const float px = posControl.activeWaypoint.pos.x; const float py = posControl.activeWaypoint.pos.y; const int32_t brgToWp = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(py - pos->y, px - pos->x))))); @@ -738,13 +717,56 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); const int8_t dirTmp = (toWpErr > 0) ? 1 : -1; const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); - // Pin the circle ahead by the roll-in drift (the FLY_BY easeLead), so the ramp - // ends ON the circle instead of leaving a radial error that slams the bank. + // Pin ahead by the roll-in drift so the ramp ends ON the circle const float leadDist = 1.5f * v * (tTmp / 1000.0f); const float cx = pos->x + leadDist * cos_approx(cogRad) + arcRtmp * cos_approx(cogRad + dirTmp * (M_PIf * 0.5f)); const float cy = pos->y + leadDist * sin_approx(cogRad) + arcRtmp * sin_approx(cogRad + dirTmp * (M_PIf * 0.5f)); + if (navConfig()->fw.wp_tracking_accuracy && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { + // Tracking ON: exit the main arc onto a BOUNDED intercept course (<= 45 deg to the + // leg, gamma = half the turn for shallow corners), short straight for the reverse + // roll, then a standard corner-cut arc rolls out ON the line + const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); + const float ux = cos_approx(legRad), uy = sin_approx(legRad); + const int32_t turnCd = wrap_18000(legBearing - cog); + const float gammaRad = CENTIDEGREES_TO_RADIANS(constrainf(0.5f * (float)ABS(turnCd), 2000.0f, 4500.0f)); + const float icptRad = legRad + (float)dirTmp * gammaRad; + const float d1x = cos_approx(icptRad), d1y = sin_approx(icptRad); + const float nAng = icptRad - (float)dirTmp * (M_PIf * 0.5f); + const float p1x = cx + 2.0f * arcRtmp * cos_approx(nAng); // intercept line shifted R toward the counter side + const float p1y = cy + 2.0f * arcRtmp * sin_approx(nAng); + const float lAng = legRad - (float)dirTmp * (M_PIf * 0.5f); + const float b0x = px + arcRtmp * cos_approx(lAng); + const float b0y = py + arcRtmp * sin_approx(lAng); + const float cross = d1x * uy - d1y * ux; + if (fabsf(cross) > 0.17f) { // gamma >= 20 deg keeps the lines well separated + const float tt = ((b0x - p1x) * uy - (b0y - p1y) * ux) / cross; + const float o2x = p1x + tt * d1x; + const float o2y = p1y + tt * d1y; + const float rollAlong = (o2x - px) * ux + (o2y - py) * uy; + const float legLen = calc_length_pythagorean_2D(px - pos->x, py - pos->y); + // roll-out tangency must lie ahead of us and leave straight leg to the WP + if (rollAlong < 0.0f && -rollAlong < legLen && -rollAlong > 2.0f * arcRtmp) { + intoEx = o2x - arcRtmp * cos_approx(nAng); // pickup = tangency on the intercept line + intoEy = o2y - arcRtmp * sin_approx(nAng); + intoO2x = o2x; intoO2y = o2y; + intoR = arcRtmp; intoBOut = legBearing; intoDir = -dirTmp; + fwArcEngaged = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = 0.0f; + arcR = arcRtmp; + arcDir = dirTmp; + arcCx = cx; + arcCy = cy; + arcOutBearing = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(icptRad)))); + phiNomCd = phiTmp; + tEaseMs = tTmp; + intoStage = FW_INTO_AWAY; // second arc staged: the shared pickup logic takes over + } + } + } const float dCP = calc_length_pythagorean_2D(px - cx, py - cy); - if (dCP > 1.05f * arcRtmp) { // next WP outside the circle: a tangent exists + if (!fwArcEngaged && dCP > 1.05f * arcRtmp) { // next WP outside the circle: a tangent exists const float alphaCP = atan2_approx(py - cy, px - cx); const float phiT = acos_approx(constrainf(arcRtmp / dCP, 0.0f, 1.0f)); for (int8_t s = -1; s <= 1; s += 2) { // of the two tangent points, exit where the tangent points at the WP @@ -776,8 +798,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / arcRtmp)); // v/R == g*tan(phi)/v const float psiTmp = 0.5f * omegaNomCds * (tTmp / 1000.0f); if (capped || ABS(hdgErr) > NAV_FW_ARC_SHARP_TURN_CD) { - // No valid tangent circle (turn started late via the cap, or near-reversal): - // fly the bounded closed-loop capture directly instead of PID / degenerate circle. + // Capped or near-reversal: no valid tangent circle - fly the bounded capture directly fwArcEngaged = true; phase = ARC_CAPTURE; fwArcBankCmd = 0.0f; // fresh engagement: don't rate-limit against a stale command @@ -797,9 +818,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) arcOutBearing = legBearing; phiNomCd = phiTmp; tEaseMs = tTmp; - // Pin the arc tangent to BOTH legs (corner-cut inscribed circle) so the exit lands ON the - // out-leg, not offset: centre = intersection of the in-leg and out-leg lines, each shifted R - // toward the turn inside. The radial term (STEADY) then converges the aircraft onto it. + // Centre = intersection of both legs shifted R inside (inscribed circle), so the exit lands ON the out-leg const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); const float d1x = cos_approx(cogRad), d1y = sin_approx(cogRad); @@ -820,12 +839,12 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } } - if (!fwArcEngaged && navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_INTO && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { + if (!fwArcEngaged && navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_BY && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { if (intoStage == FW_INTO_MAIN) { // crossing flown: re-arm once the leg has switched (normally it already has, mid-arc) intoStage = (ABS(wrap_18000(legBearing - intoBOut)) < 500) ? FW_INTO_IDLE : FW_INTO_DONE; } - if (intoStage == FW_INTO_IDLE) { + if (intoStage == FW_INTO_IDLE && navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_INTO) { const int32_t nta = posControl.activeWaypoint.nextTurnAngle; if (nta != -1 && ABS(nta) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); @@ -836,10 +855,8 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float bOutRad = CENTIDEGREES_TO_RADIANS((float)bOut); const float bInRad = CENTIDEGREES_TO_RADIANS((float)legBearing); const float ux = cos_approx(bInRad), uy = sin_approx(bInRad); - // Main circle pinned to the WP + outbound course; counter circle tangent to the - // inbound leg on the opposite side. Centers spaced sqrt((2R)^2 + Ls^2): an internal- - // tangent gap Ls stays between the arcs as room for the roll swing that touching - // circles would demand instantaneously. + // Main circle pinned at the WP, counter circle on the inbound leg; centers + // sqrt((2R)^2+Ls^2) apart so an Ls gap gives the roll swing room const float Ls = 2.0f * v * (tTmp / 1000.0f); const float o2x = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(bOutRad + dirM * (M_PIf * 0.5f)); const float o2y = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(bOutRad + dirM * (M_PIf * 0.5f)); @@ -878,8 +895,10 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } } else if (intoStage == FW_INTO_AWAY) { - // away-arc handed back early (aligned at the inflection course): pick up the main arc - const float distI = calc_length_pythagorean_2D(intoEx - pos->x, intoEy - pos->y); + // first arc handed back on the tangent course: pick up the second arc. Along-track + // projection, not proximity - a lateral residual must not make us miss the point. + const float outRad = CENTIDEGREES_TO_RADIANS((float)arcOutBearing); + const float distI = (intoEx - pos->x) * cos_approx(outRad) + (intoEy - pos->y) * sin_approx(outRad); const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * intoR))); const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); if (distI <= 1.5f * v * (tTmp / 1000.0f)) { @@ -902,26 +921,51 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) return; } } else { - // Mission advanced mid-arc (short leg): retarget the closed-loop capture onto the new leg instead - // of finishing the turn onto the stale out-bearing. The capture law is bounded (+/- phiNom) and - // hands back once aligned, so consecutive quick corners degrade gracefully instead of being skipped. + // Mission advanced mid-arc: retarget the bounded capture onto the new leg instead of the stale out-bearing if (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500) { fwFlyByCappedLatch = false; arcOutBearing = legBearing; phase = ARC_CAPTURE; + if (intoStage == FW_INTO_AWAY) { + intoStage = FW_INTO_DONE; // staged S is stale: release the hand-back block + } } fwArcPrevLegBearing = legBearing; - // FLY_INTO fallback: away arc still engaged at the pickup point (capture has not handed back) - swing over directly + // Pick up the second arc at its tangency point. Along-track distance so a lateral residual + // cannot miss it; the heading gate blocks the trigger early in the first arc, where the + // pickup point still lies behind the exit course. + const float outRadE = CENTIDEGREES_TO_RADIANS((float)arcOutBearing); if (intoStage == FW_INTO_AWAY - && calc_length_pythagorean_2D(intoEx - pos->x, intoEy - pos->y) <= 1.5f * v * (tEaseMs / 1000.0f)) { + && ABS(wrap_18000(arcOutBearing - cog)) < 4500 + && ((intoEx - pos->x) * cos_approx(outRadE) + (intoEy - pos->y) * sin_approx(outRadE)) <= 1.5f * v * (tEaseMs / 1000.0f)) { phase = ARC_RAMP_IN; rampMs = 0.0f; - rampStartCd = fwArcBankCmd; // swing -phi -> +phi through the inflection - arcR = intoR; + rampStartCd = fwArcBankCmd; // blend from the current bank arcDir = intoDir; - arcCx = intoO2x; - arcCy = intoO2y; + // Radius from CURRENT groundspeed (the arming value may be unflyable downwind), and the + // circle re-anchored along the leg line through the actual position: wind drift becomes + // an along-track shift instead of a parallel roll-out offset + const float r2 = getFwCoordinatedTurnRadius(); + const float legR2 = CENTIDEGREES_TO_RADIANS((float)intoBOut); + const float u2x = cos_approx(legR2), u2y = sin_approx(legR2); + const float b0x = posControl.activeWaypoint.pos.x + r2 * cos_approx(legR2 + (float)intoDir * (M_PIf * 0.5f)); + const float b0y = posControl.activeWaypoint.pos.y + r2 * sin_approx(legR2 + (float)intoDir * (M_PIf * 0.5f)); + const float w2x = pos->x - b0x, w2y = pos->y - b0y; + const float w2u = w2x * u2x + w2y * u2y; + const float disc2 = w2u * w2u - (w2x * w2x + w2y * w2y) + r2 * r2; + if (disc2 > 0.0f) { + const float t2 = w2u + sqrtf(disc2); + arcR = r2; + arcCx = b0x + t2 * u2x; + arcCy = b0y + t2 * u2y; + phiNomCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * r2))); + tEaseMs = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiNomCd)); + } else { // drifted beyond the line: keep the planned circle + arcR = intoR; + arcCx = intoO2x; + arcCy = intoO2y; + } arcOutBearing = intoBOut; intoStage = FW_INTO_MAIN; } @@ -930,9 +974,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) rampMs += US2S(deltaMicros) * 1000.0f; const int32_t hdgErrOut = wrap_18000(arcOutBearing - cog); - // Roll-out prediction, shared by STEADY (early capture hand-over) and CAPTURE (lead): heading - // consumed by the shaped down-ramp (0.5*omega*tEase, smoothstep integral) plus the angle-P - // response tail (tau = 1/(LEVEL_P * multiplier)) and the unmodelled servo/aero delay. + // Roll-out lead: heading consumed by the shaped down-ramp plus the angle-P tail and servo delay const float bankNowRad = CENTIDEGREES_TO_RADIANS((float)ABS(attitude.values.roll) * 10.0f); const float omegaCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(GRAVITY_CMSS * tan_approx(bankNowRad) / MAX(v, NAV_FW_TURN_MIN_SPEED))); const float levelGain = pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; // [1/s] @@ -941,7 +983,10 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) switch (phase) { case ARC_RAMP_IN: { - const float p = (tEaseMs > 1.0f) ? constrainf(rampMs / tEaseMs, 0.0f, 1.0f) : 1.0f; + // Rate-consistent: a swing spanning 2*phi takes twice the standard ease time + const float rampSpanCd = fabsf((float)arcDir * phiNomCd - rampStartCd); + const float rampDurMs = MAX(tEaseMs * rampSpanCd / MAX(phiNomCd, 1.0f), 0.5f * tEaseMs); + const float p = (rampDurMs > 1.0f) ? constrainf(rampMs / rampDurMs, 0.0f, 1.0f) : 1.0f; const float s = p * p * (3.0f - 2.0f * p); // smoothstep up fwArcBankCmd = rampStartCd + ((float)arcDir * phiNomCd - rampStartCd) * s; if (p >= 1.0f) { // roll-in done -> track the pre-placed tangent circle @@ -956,8 +1001,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float alpha = atan2_approx(dy, dx); // azimuth on the arc const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(arcDir * cos_approx(alpha), -arcDir * sin_approx(alpha))))); const int32_t eH = wrap_18000(tangentBearing - cog); // [centideg] heading error to the arc tangent - // Feed-forward from CURRENT groundspeed: wind changes v along the arc, so the frozen - // engagement bank would leave the radial feedback carrying the whole v^2 shift. + // FF from current groundspeed: wind changes v along the arc const float phiLiveCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcR))); fwArcBankCmd = arcDir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwArcBankCmd) @@ -968,9 +1012,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } case ARC_CAPTURE: default: { - // Closed-loop roll-out: the no-overshoot envelope (bank proportional to the led remaining - // heading), with the command's collapse rate-limited to the entry ramp's build-up rate so - // the level-off is eased instead of an angle-P slam. Magnitude growth stays unrestricted. + // No-overshoot envelope; the collapse is rate-limited to the ramp rate so the level-off is eased int32_t captureErr = hdgErrOut; if (ABS(captureErr) > 17000) { captureErr = arcDir * ABS(captureErr); // ambiguous reversal: hold the engagement direction @@ -984,7 +1026,10 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) cmd = MIN(cmd, fwArcBankCmd + maxStepCd); } fwArcBankCmd = cmd; - if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwArcBankCmd) <= phiNomCd * 0.1f) { // aligned and nearly level -> hand back + // Mid-S the gap between the arcs stays engaged: handing back there would give the PID and + // path tracking a moment of control while we sit a full turn diameter off the leg. + if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwArcBankCmd) <= phiNomCd * 0.1f + && intoStage != FW_INTO_AWAY) { // aligned and nearly level -> hand back fwArcEngaged = false; return; } @@ -992,6 +1037,11 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } + // Clamp to the flyable ceiling: rate limits, handoff checks and the smoother seed must not + // run on a command the airframe cannot reach (wind can drive eR arbitrarily large) + const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); + fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); + DEBUG_SET(DEBUG_FW_TURN, 2, arcOutBearing); // exit course while the arc is active; FLY_OVER: tangent through the next WP DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); // bank command [centideg], all phases DEBUG_SET(DEBUG_FW_TURN, 5, hdgErrOut); // remaining heading to out-leg [centideg]; closed-loop capture -> 0 (should not overshoot) @@ -1047,9 +1097,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && !flyIntoMissionLeg && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { const float turnRadius = getFwCoordinatedTurnRadius(); const float halfAngleTan = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 0.0f, NAV_FW_TURN_LEAD_TAN_MAX); - // Roll-in lead: the smoothstep ramp is back-loaded AND cog (ground track) lags the bank, so the aircraft - // flies nearly straight for longer than the ramp lasts -> the steady arc begins well downrange. Lead by - // k*V*T_in. T_in from roll rate + control_smoothness + control_ease. Coefficient calibrated from flight. + // Roll-in lead: the ramp is back-loaded and the ground track lags the bank (k calibrated in flight) const float easeLeadDistance = posControl.actualState.velXY * (fwTurnEaseTimeMs(getFwPlanningBankDeg()) / 1000.0f) * 1.5f; float turnStartDistance = easeLeadDistance + turnRadius * halfAngleTan; // Cap how early the turn may begin (nav_fw_wp_turn_max_lead_time): never more than N ms of flight before the WP. @@ -1077,8 +1125,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY); } - // Arc turn coordinator (Approach B): manages the turn state and commands the roll bank directly - // (applied in updatePositionHeadingController_FW). The position carrot stays on the normal path. + // Arc turn coordinator: manages the turn state and commands the roll bank directly updateFwTurnArc(deltaMicros); // Calculate virtual waypoint @@ -1197,12 +1244,18 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); } } else { - /* Keep state synced to the current error while not steering, so the - * controller re-engages cleanly on the next leg (no stale-data kick). */ + /* Keep state synced to the current error while not steering, and seed the convergence + * estimate from the geometric closing speed: re-engaging with a zero rate reads as + * "not converging" and commands the full correction in one step (arc hand-back kick). */ previousCrossTrackError = navCrossTrackError; previousCrossTrackErrorUpdateTime = currentTimeUs; - crossTrackErrorRate = 0.0f; - pt1FilterReset(&fwCrossTrackErrorRateFilterState, 0.0f); + const fpVector3_t *trackPos = &navGetCurrentActualPositionAndVelocity()->pos; + const float legRad = CENTIDEGREES_TO_RADIANS((float)posControl.activeWaypoint.bearing); + const float offLeg = (trackPos->x - virtualCoursePoint.x) * (-sin_approx(legRad)) + + (trackPos->y - virtualCoursePoint.y) * cos_approx(legRad); + const float cogOffRad = CENTIDEGREES_TO_RADIANS((float)wrap_18000(posControl.actualState.cog - posControl.activeWaypoint.bearing)); + crossTrackErrorRate = -SIGN(offLeg) * posControl.actualState.velXY * sin_approx(cogOffRad); + pt1FilterReset(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate); } } /* @@ -1236,8 +1289,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta } // Only allow PID integrator to shrink if error is decreasing over time. - // While the arc coordinator drives the turn the FF sets the bank and the carrot-P does the tracking, - // so freeze the integrator: otherwise it winds up (carrot error keeps one sign) and slams the turn at handback. + // Freeze the integrator while the arc drives the turn - the carrot error keeps one sign and winds it up const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0) | (fwArcActive ? PID_FREEZE_INTEGRATOR : 0); @@ -1249,11 +1301,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta DEGREES_TO_CENTIDEGREES(navBankLimit), pidFlags); - // Arc turn coordinator drives the roll directly while active (overrides the PID; FF is skipped, the - // arc bank command already is the coordinated bank). Its smoothstep ramps are already gentle, so - // control_smoothness is bypassed during the arc (CS is folded into the ease time instead); the smoother - // is re-seeded on the first direct frame after an arc handback or a controller reset so a stale - // internal state cannot smear or falsely trigger the S-curve. + // Arc bank overrides the PID; its ramps are already shaped, so the S-curve smoother is bypassed + // and re-seeded at handback to avoid a stale-state step if (fwArcActive) { rollAdjustment = fwArcBankCmd; fwRollSmoothSeedCd = fwArcBankCmd; // else the handback re-seeds from a stale reset value (brief roll twitch) From 8621aad210f9363c62280a101ac3258dee5c7984 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Sat, 22 Aug 2026 14:17:26 +0200 Subject: [PATCH 16/21] FW nav: consolidate WP turn settings into nav_fw_wp_turn_mode, release debug layout - nav_fw_wp_turn_coordination merged into nav_fw_wp_turn_mode (DIRECT / COORD_FLYBY / COORD_FLYOVER / COORD_FLYINTO), PG_NAV_CONFIG -> 14 - DEBUG_FW_TURN: one owner per channel, documented in docs/development/fw-turn-debugging.md for the official docs - remove unreachable not-engaged S pickup branch (superseded by the engaged pickup; defensive stale release on controller reset mid-S) Co-Authored-By: Claude Fable 5 --- docs/Settings.md | 22 ++------ docs/development/fw-turn-debugging.md | 57 +++++++++++++++++++ src/main/fc/settings.yaml | 16 ++---- src/main/navigation/navigation.c | 7 +-- src/main/navigation/navigation.h | 15 ++--- src/main/navigation/navigation_fixedwing.c | 65 +++++++--------------- 6 files changed, 94 insertions(+), 88 deletions(-) create mode 100644 docs/development/fw-turn-debugging.md diff --git a/docs/Settings.md b/docs/Settings.md index 6fec18a10a6..5c0efd79be4 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4189,20 +4189,9 @@ DEVELOPER/EXPERIMENTAL: unmodelled roll-response lag (servo + airframe inertia) --- -### nav_fw_wp_turn_coordination - -How FW waypoint turns are flown. COORDINATED (default) commands an explicit coordinated arc of the planned radius at nav_fw_bank_angle and tracks it to the next leg. DIRECT uses the legacy heading-PID turn (fallback for users who prefer the old behaviour). - -| Allowed Values | | -| --- | --- | -| DIRECT | | -| COORDINATED | Default | - ---- - ### nav_fw_wp_turn_max_lead_time -FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint. +COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint. | Default | Min | Max | | --- | --- | --- | @@ -4212,13 +4201,14 @@ FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. The req ### nav_fw_wp_turn_mode -How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint - or, with nav_fw_wp_tracking_accuracy enabled, an S-turn that rolls out directly ON the new leg. FLY_INTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries). +How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn (fallback for users who prefer the old behaviour). The COORD modes command an explicit coordinated arc of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY anticipates the turn so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). COORD_FLYOVER flies over the waypoint, then rolls out exactly on the tangent line to the next waypoint - or, with nav_fw_wp_tracking_accuracy enabled, an S-turn that rolls out directly ON the new leg. COORD_FLYINTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries). | Allowed Values | | | --- | --- | -| FLY_BY | Default | -| FLY_OVER | | -| FLY_INTO | | +| DIRECT | | +| COORD_FLYBY | Default | +| COORD_FLYOVER | | +| COORD_FLYINTO | | --- diff --git a/docs/development/fw-turn-debugging.md b/docs/development/fw-turn-debugging.md new file mode 100644 index 00000000000..a152eb76f79 --- /dev/null +++ b/docs/development/fw-turn-debugging.md @@ -0,0 +1,57 @@ +# Fixed-wing coordinated turn debugging (`DEBUG_FW_TURN`) + +`set debug_mode = FW_TURN` exposes the fixed-wing coordinated WP turn system: the arc turn +coordinator, the S sequencer (COORD_FLYOVER with path tracking / COORD_FLYINTO), the loiter +radius stabiliser, the turn/loiter feed-forward and the energy bank guard. Values are available +via the CLI `debug` command, the `OSD_DEBUG` element, MSP `DEBUGMSG`/`DEBUG` telemetry and +blackbox logging. + +## Channels + +| debug[] | Value | Unit | Written by | +|---|---|---|---| +| 0 | Active turn/loiter radius | cm | Loiter stabiliser every cycle; overridden by the COORD_FLYBY planning radius while a corner approach is active, and by the engaged arc's radius while the coordinator flies the turn (latest writer wins) | +| 1 | Coordinator state (see below) | – | Arc coordinator | +| 2 | Exit course of the active arc | centideg | Arc coordinator, only while engaged | +| 3 | Remaining heading to the exit course | centideg | Arc coordinator, only while engaged | +| 4 | Arc bank command | centideg | Arc coordinator, only while engaged; clamped to the effective bank ceiling | +| 5 | Turn/loiter roll feed-forward | centideg | Feed-forward (0 when disabled, not established on the loiter circle, or inside the heading deadband) | +| 6 | Energy-guard bank ceiling | deg | Energy bank guard (sits at `max_angle_inclination_rll` unless the guard is reducing it) | +| 7 | Roll ease time | ms | Arc coordinator, only while engaged (sizes the entry/exit ramps and the turn-start lead) | + +Channels 2, 3, 4 and 7 hold their last value after the arc disengages; check channel 1 to know +whether the coordinator is active. + +## Channel 1: coordinator state + +While the arc coordinator is engaged the value is `(arc phase + 1) * 10 + S stage`; while idle +it is the S stage alone. + +Arc phase (tens digit): + +| Digit | Phase | Meaning | +|---|---|---| +| 1 | RAMP_IN | Smoothstep bank ramp onto the pre-placed circle | +| 2 | STEADY | Coordinated arc: live feed-forward bank + radius/tangent feedback | +| 3 | CAPTURE | Predictive roll-out onto the exit course | + +S stage (ones digit; 0 outside the S modes): + +| Digit | Stage | Meaning | +|---|---|---| +| 0 | – | Plain single-arc turn (COORD_FLYBY, COORD_FLYOVER tangent exit) | +| 1 | AWAY | First arc of the S; the second arc is staged | +| 2 | MAIN | Second arc of the S (COORD_FLYINTO: the aligned WP crossing; COORD_FLYOVER + tracking: the corner-cut onto the leg) | +| 3 | DONE | S completed for this leg, waiting for the next leg switch | + +Examples: `20` = flying a plain coordinated turn; `21` = steady on the first S arc; `32` = +rolling out of the S's second arc; `3` (idle) = S finished, coordinator handed back. + +## Reading a turn + +A healthy COORD_FLYBY corner shows: channel 1 stepping `10 → 20 → 30 → 0`, channel 4 ramping to +the nominal bank, holding, then collapsing as channel 3 converges to 0 without overshoot. The +hand-back happens aligned (|ch3| small) and nearly level (|ch4| < 10% of nominal). Channel 4 +pegged at channel 6 × 100 indicates the command is saturated at the flyable ceiling — expected +briefly downwind, a problem if sustained. Channel 6 dropping below `max_angle_inclination_rll` +means the energy guard is trading bank for climb capability. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f3473117ae3..0a2bd2445ae 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -181,11 +181,8 @@ tables: values: ["2D", "3D"] enum: dynamicGyroNotchMode_e - name: nav_fw_wp_turn_mode - values: ["FLY_BY", "FLY_OVER", "FLY_INTO"] + values: ["DIRECT", "COORD_FLYBY", "COORD_FLYOVER", "COORD_FLYINTO"] enum: navFwWpTurnMode_e - - name: nav_fw_wp_turn_coordination - values: ["DIRECT", "COORDINATED"] - enum: navFwWpTurnCoordination_e - name: gps_auto_baud_max values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600'] enum: gpsBaudRate_e @@ -2674,8 +2671,8 @@ groups: min: 30 max: 80 - name: nav_fw_wp_turn_mode - description: "How the aircraft turns at waypoints during FW WP missions. FLY_BY anticipates the turn using the real coordinated-turn radius (from speed and nav_fw_bank_angle) so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). FLY_OVER flies over the waypoint, then flies a coordinated turn that rolls out exactly on the tangent line to the next waypoint - or, with nav_fw_wp_tracking_accuracy enabled, an S-turn that rolls out directly ON the new leg. FLY_INTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries)." - default_value: "FLY_BY" + description: "How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn (fallback for users who prefer the old behaviour). The COORD modes command an explicit coordinated arc of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY anticipates the turn so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). COORD_FLYOVER flies over the waypoint, then rolls out exactly on the tangent line to the next waypoint - or, with nav_fw_wp_tracking_accuracy enabled, an S-turn that rolls out directly ON the new leg. COORD_FLYINTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries)." + default_value: "COORD_FLYBY" field: fw.wp_turn_mode table: nav_fw_wp_turn_mode - name: nav_fw_turn_ff_gain @@ -2684,13 +2681,8 @@ groups: field: fw.turn_ff_gain min: 0 max: 200 - - name: nav_fw_wp_turn_coordination - description: "How FW waypoint turns are flown. COORDINATED (default) commands an explicit coordinated arc of the planned radius at nav_fw_bank_angle and tracks it to the next leg. DIRECT uses the legacy heading-PID turn (fallback for users who prefer the old behaviour)." - default_value: "COORDINATED" - field: fw.wp_turn_coordination - table: nav_fw_wp_turn_coordination - name: nav_fw_wp_turn_max_lead_time - description: "FLY_BY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint." + description: "COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint." default_value: 6000 field: fw.wp_turn_max_lead_time min: 3000 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 1d2ac8f64b6..3063ef33d9c 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, 13); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -250,9 +250,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT, // pitch angle mode deadband when Saoring mode enabled .wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions .wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs - .wp_turn_mode = SETTING_NAV_FW_WP_TURN_MODE_DEFAULT, // FLY_BY, WP mission turn mode + .wp_turn_mode = SETTING_NAV_FW_WP_TURN_MODE_DEFAULT, // COORD_FLYBY, WP mission turn mode .turn_ff_gain = SETTING_NAV_FW_TURN_FF_GAIN_DEFAULT, // 100, turn FF - .wp_turn_coordination = SETTING_NAV_FW_WP_TURN_COORDINATION_DEFAULT, // COORDINATED, arc-based turns .wp_turn_max_lead_time = SETTING_NAV_FW_WP_TURN_MAX_LEAD_TIME_DEFAULT, // 3000 ms .wp_turn_control_ease = SETTING_NAV_FW_WP_TURN_CONTROL_EASE_DEFAULT, // 100 ms } @@ -4305,7 +4304,7 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint) calculateAndSetActiveWaypointToLocalPosition(&localPos); // Turn anticipation (nextTurnAngle) is needed for FLY_BY and FLY_INTO; FLY_OVER flies to the WP then turns. - if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_OVER) { + if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_COORD_FLY_OVER) { fpVector3_t posNextWp; if (getLocalPosNextWaypoint(&posNextWp)) { int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index a9f65f01ea4..a06cc993480 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -334,16 +334,12 @@ typedef enum { RTH_TRACKBACK_FS, } rthTrackbackMode_e; -typedef enum { - NAV_FW_WP_TURN_MODE_FLY_BY = 0, // corner cut: turn anticipated so the arc joins the next leg, WP passed abeam - NAV_FW_WP_TURN_MODE_FLY_OVER = 1, // fly over the WP, then roll out on the tangent line to the next WP - NAV_FW_WP_TURN_MODE_FLY_INTO = 2, // ease away before the WP, then cross it already aligned on the outbound course -} navFwWpTurnMode_e; - typedef enum { NAV_FW_WP_TURN_DIRECT = 0, // legacy heading-PID turn (fallback) - NAV_FW_WP_TURN_COORDINATED = 1, // arc-based coordinated turn (default) -} navFwWpTurnCoordination_e; + NAV_FW_WP_TURN_COORD_FLY_BY = 1, // corner cut: turn anticipated so the arc joins the next leg, WP passed abeam + NAV_FW_WP_TURN_COORD_FLY_OVER = 2, // fly over the WP, then roll out on the tangent line to the next WP + NAV_FW_WP_TURN_COORD_FLY_INTO = 3, // ease away before the WP, then cross it already aligned on the outbound course +} navFwWpTurnMode_e; typedef enum { MC_ALT_HOLD_STICK, @@ -512,9 +508,8 @@ typedef struct navConfig_s { uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg) uint8_t wp_tracking_accuracy; // fixed wing tracking accuracy response factor uint8_t wp_tracking_max_angle; // fixed wing tracking accuracy max alignment angle [degs] - uint8_t wp_turn_mode; // WP mission turn mode (navFwWpTurnMode_e: FLY_BY / FLY_OVER) + uint8_t wp_turn_mode; // WP mission turn mode (navFwWpTurnMode_e: DIRECT / COORD_FLYBY / COORD_FLYOVER / COORD_FLYINTO) uint8_t turn_ff_gain; // turn coordination feed-forward gain [%] (0 = off; dev tuning, to be hardcoded) - uint8_t wp_turn_coordination; // turn handling (navFwWpTurnCoordination_e: DIRECT / COORDINATED) uint16_t wp_turn_max_lead_time; // FLY_BY: cap on how early the turn may start before the WP [ms] (dev tuning) uint16_t wp_turn_control_ease; // unmodelled roll-response lag added to the computed turn ease time [ms] } fw; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 8167ed31d33..e68e9044d29 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -535,10 +535,7 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl } fwEffectiveBankLimit = constrainf(fwEffectiveBankLimit, NAV_FW_GUARD_PHI_FLOOR_DEG, maxBank); - if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_BY) { // ch1 owned by the S-sequencer stage diagnostic otherwise - DEBUG_SET(DEBUG_FW_TURN, 1, lrintf(fwEffectiveBankLimit)); - } - // ch4/5/6 temporarily owned by the arc coordinator diagnostic (guard deficit/rise/trigger muted) + DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(fwEffectiveBankLimit)); // energy-guard bank ceiling [deg] } // Coordinated-turn radius R = V^2/(g*tan(phi)) [cm], clamped. Times the FLY_BY turn for any speed. @@ -586,7 +583,7 @@ static float getFwTurnFeedForward(int32_t navHeadingError) const float phiFFcd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * ffRadius))); rollFF = ffSign * phiFFcd * (ffGain / 100.0f); } - DEBUG_SET(DEBUG_FW_TURN, 7, lrintf(rollFF)); + DEBUG_SET(DEBUG_FW_TURN, 5, lrintf(rollFF)); // turn/loiter roll feed-forward [centideg] return rollFF; } @@ -639,9 +636,7 @@ static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearing } const uint32_t out = (uint32_t)MAX((float)configuredRadius, commandedHold); - DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(out)); // commanded loiter radius (stabilised) - DEBUG_SET(DEBUG_FW_TURN, 2, lrintf(required)); // instantaneous required (swings with wind) - DEBUG_SET(DEBUG_FW_TURN, 3, lrintf(speed)); // ground speed + DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(out)); // active turn/loiter radius [cm] (overridden by FLY_BY/arc writers) return out; } @@ -669,7 +664,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) static float rampMs; // elapsed time in the ramp-in phase static float rampStartCd; // bank the ramp blends from (0 on entry; -phi at the FLY_INTO inflection) - // S sequencer (FLY_INTO / FLY_OVER-tracking): first arc, internal-tangent gap, second arc + // S sequencer (FLY_INTO / FLY_OVER-tracking): first arc, roll-reversal gap, second arc enum { FW_INTO_IDLE = 0, FW_INTO_AWAY, FW_INTO_MAIN, FW_INTO_DONE }; static uint8_t intoStage; static float intoEx, intoEy; // second-arc pickup point (internal-tangent touch) @@ -681,7 +676,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) fwArcActive = false; const bool wpTracking = isWaypointNavTrackingActive() && !needToCalculateCircularLoiter; - if (navConfig()->fw.wp_turn_coordination != NAV_FW_WP_TURN_COORDINATED || !wpTracking) { + if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_DIRECT || !wpTracking) { fwArcEngaged = false; fwArcPrevLegBearing = -1; intoStage = FW_INTO_IDLE; @@ -693,10 +688,6 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; const float v = posControl.actualState.velXY; - if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_BY) { - DEBUG_SET(DEBUG_FW_TURN, 1, intoStage); // S-sequencer stage (FLY_INTO / FLY_OVER-tracking diagnostics) - } - if (!fwArcEngaged) { const bool legChanged = (fwArcPrevLegBearing >= 0) && (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500); fwArcPrevLegBearing = legBearing; @@ -704,7 +695,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) intoStage = FW_INTO_IDLE; // a new leg invalidates any staged S geometry const bool capped = fwFlyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits fwFlyByCappedLatch = false; // consume the latch on any leg change - if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_OVER) { + if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_OVER) { // FLY_OVER: circle pinned at the overfly point. Tracking OFF: exit on the tangent // through the next WP; tracking ON: bounded-intercept S onto the new leg itself. const float px = posControl.activeWaypoint.pos.x; @@ -791,7 +782,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } const int32_t hdgErr = wrap_18000(legBearing - cog); - if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_OVER && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_COORD_FLY_OVER && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); @@ -839,12 +830,12 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } } - if (!fwArcEngaged && navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_MODE_FLY_BY && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { + if (!fwArcEngaged && navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_COORD_FLY_BY && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { if (intoStage == FW_INTO_MAIN) { // crossing flown: re-arm once the leg has switched (normally it already has, mid-arc) intoStage = (ABS(wrap_18000(legBearing - intoBOut)) < 500) ? FW_INTO_IDLE : FW_INTO_DONE; } - if (intoStage == FW_INTO_IDLE && navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_INTO) { + if (intoStage == FW_INTO_IDLE && navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_INTO) { const int32_t nta = posControl.activeWaypoint.nextTurnAngle; if (nta != -1 && ABS(nta) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); @@ -895,29 +886,11 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } } else if (intoStage == FW_INTO_AWAY) { - // first arc handed back on the tangent course: pick up the second arc. Along-track - // projection, not proximity - a lateral residual must not make us miss the point. - const float outRad = CENTIDEGREES_TO_RADIANS((float)arcOutBearing); - const float distI = (intoEx - pos->x) * cos_approx(outRad) + (intoEy - pos->y) * sin_approx(outRad); - const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * intoR))); - const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); - if (distI <= 1.5f * v * (tTmp / 1000.0f)) { - fwArcEngaged = true; - phase = ARC_RAMP_IN; - rampMs = 0.0f; - rampStartCd = 0.0f; - arcR = intoR; - arcDir = intoDir; - arcCx = intoO2x; - arcCy = intoO2y; - arcOutBearing = intoBOut; - phiNomCd = phiTmp; - tEaseMs = tTmp; - intoStage = FW_INTO_MAIN; - } + intoStage = FW_INTO_IDLE; // only reachable via a controller reset mid-S: geometry is stale } } if (!fwArcEngaged) { + DEBUG_SET(DEBUG_FW_TURN, 1, intoStage); return; } } else { @@ -1042,10 +1015,12 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); - DEBUG_SET(DEBUG_FW_TURN, 2, arcOutBearing); // exit course while the arc is active; FLY_OVER: tangent through the next WP - DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); // bank command [centideg], all phases - DEBUG_SET(DEBUG_FW_TURN, 5, hdgErrOut); // remaining heading to out-leg [centideg]; closed-loop capture -> 0 (should not overshoot) - DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(tEaseMs)); // roll-in ease time [ms] -> sizes the turn-start lead (V*tEase) + DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(arcR)); // active arc radius [cm] + DEBUG_SET(DEBUG_FW_TURN, 1, (phase + 1) * 10 + intoStage); // coordinator state: (arc phase + 1)*10 + S stage + DEBUG_SET(DEBUG_FW_TURN, 2, arcOutBearing); // exit course [centideg] + DEBUG_SET(DEBUG_FW_TURN, 3, hdgErrOut); // remaining heading to the exit course [centideg] + DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); // arc bank command [centideg] + DEBUG_SET(DEBUG_FW_TURN, 7, lrintf(tEaseMs)); // roll ease time [ms] -> sizes the turn leads fwArcActive = true; } @@ -1093,7 +1068,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t * at any speed. Only runs when nextTurnAngle is set (FLY_BY waypoints + landing); FLY_OVER skips it. */ int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); posControl.flags.wpTurnSmoothingActive = false; - const bool flyIntoMissionLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_MODE_FLY_INTO && (navGetCurrentStateFlags() & NAV_AUTO_WP); + const bool flyIntoMissionLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_INTO && (navGetCurrentStateFlags() & NAV_AUTO_WP); if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && !flyIntoMissionLeg && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { const float turnRadius = getFwCoordinatedTurnRadius(); const float halfAngleTan = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 0.0f, NAV_FW_TURN_LEAD_TAN_MAX); @@ -1104,9 +1079,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t const float maxLeadDistance = posControl.actualState.velXY * (float)navConfig()->fw.wp_turn_max_lead_time * 0.001f; const bool turnCapped = turnStartDistance > maxLeadDistance; turnStartDistance = MIN(turnStartDistance, maxLeadDistance); - DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(turnRadius)); - DEBUG_SET(DEBUG_FW_TURN, 2, lrintf(turnStartDistance)); - DEBUG_SET(DEBUG_FW_TURN, 3, lrintf(posControl.wpDistance)); + DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(turnRadius)); // FLY_BY planning radius while approaching if (posControl.wpDistance < turnStartDistance) { posControl.flags.wpTurnSmoothingActive = true; fwFlyByCappedLatch = turnCapped; // capped corner cut -> the arc coordinator flies it direct instead From 66620e0334dcbe54af678d81de1cf9e0dc8c0f28 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Sat, 22 Aug 2026 14:32:14 +0200 Subject: [PATCH 17/21] FW nav: leg-line capture - fallback turns converge onto the track when path tracking is on Reversal/capped fallbacks ended parallel to the leg a full turn-diameter off (logged: 116 m after the 180 at WP1, 46 deg tracker cut). The capture target now tracks a live intercept course onto the leg line, tapering 1 cd/cm and capped at nav_fw_wp_tracking_max_angle. Sim: handoff on ~50 deg intercept, on the line within 0.4-4 m. Co-Authored-By: Claude Fable 5 --- src/main/navigation/navigation_fixedwing.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e68e9044d29..188e977ab2c 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -659,6 +659,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) static float arcCx, arcCy, arcR; static int8_t arcDir; static int32_t arcOutBearing; + static bool arcToLegLine; // fallback capture: converge onto the leg line itself, not just its course static float phiNomCd; // coordinated nominal bank for this turn [centideg] static float tEaseMs; // roll-in ease time static float rampMs; // elapsed time in the ramp-in phase @@ -689,6 +690,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) const float v = posControl.actualState.velXY; if (!fwArcEngaged) { + arcToLegLine = false; const bool legChanged = (fwArcPrevLegBearing >= 0) && (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500); fwArcPrevLegBearing = legBearing; if (legChanged) { @@ -797,6 +799,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) rampStartCd = 0.0f; arcDir = (hdgErr > 0) ? 1 : -1; arcOutBearing = legBearing; + arcToLegLine = true; phiNomCd = phiTmp; tEaseMs = tTmp; } else if (2.0f * psiTmp < (float)ABS(hdgErr)) { // enough turn left for a steady arc between the ease ramps @@ -898,6 +901,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) if (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500) { fwFlyByCappedLatch = false; arcOutBearing = legBearing; + arcToLegLine = true; phase = ARC_CAPTURE; if (intoStage == FW_INTO_AWAY) { intoStage = FW_INTO_DONE; // staged S is stale: release the hand-back block @@ -944,6 +948,17 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } + // Leg-line capture (path tracking on): steer onto the track itself, not merely parallel to it - + // a reversal fallback otherwise ends a turn-diameter off the leg. Intercept angle tapers with + // the cross-track offset (1 cd/cm), capped at the tracker's own convergence limit. + if (arcToLegLine && navConfig()->fw.wp_tracking_accuracy) { + const float legRadT = CENTIDEGREES_TO_RADIANS((float)legBearing); + const float offLeg = (pos->x - posControl.activeWaypoint.pos.x) * (-sin_approx(legRadT)) + + (pos->y - posControl.activeWaypoint.pos.y) * cos_approx(legRadT); + const float gammaCd = constrainf(fabsf(offLeg), 0.0f, DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle)); + arcOutBearing = wrap_36000(legBearing - lrintf(SIGN(offLeg) * gammaCd)); + } + rampMs += US2S(deltaMicros) * 1000.0f; const int32_t hdgErrOut = wrap_18000(arcOutBearing - cog); From 681c95173a20ef6f1a84c06751af0e33f37d3fbf Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Sat, 22 Aug 2026 15:00:31 +0200 Subject: [PATCH 18/21] FW nav: autoland approach always flies coordinated FLY_BY turns The arc coordinator followed the global nav_fw_wp_turn_mode on the landing approach legs; clean transitions between the approach tracks are mandatory, so the effective mode is forced to COORD_FLYBY there (from DIRECT too). Landing doc updated - approach turning points come from the coordinated turn radius, not nav_wp_radius; turn-mode setting description shortened. Co-Authored-By: Claude Fable 5 --- docs/Fixed Wing Landing.md | 6 ++++-- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_fixedwing.c | 19 +++++++++++++------ 4 files changed, 19 insertions(+), 10 deletions(-) diff --git a/docs/Fixed Wing Landing.md b/docs/Fixed Wing Landing.md index 7f4ce455f78..9b5d28c608f 100644 --- a/docs/Fixed Wing Landing.md +++ b/docs/Fixed Wing Landing.md @@ -20,6 +20,8 @@ This enables up to 4 different approach directions, based on the landing site an 7. Flare: Only if a LIDAR/Rangefinder sensor is present: the motor remains switched off and the pitch angle of "Flare Pitch" is held 8. Landing: As soon as INAV has detected the landing, it is automatically disarmed, see setting `nav_disarm_on_landing`. +All turns between the approach legs are flown as coordinated FLY_BY corner cuts: the turn start is anticipated from the aircraft's actual turn radius (speed and `nav_fw_bank_angle`) so the plane rolls out already aligned on the next leg. This applies during the landing approach regardless of the configured `nav_fw_wp_turn_mode`. + To activate the automatic landing, the parameter `nav_rth_allow_landing` must be set to `ALWAYS` or `FAILSAFE`. > [!WARNING] @@ -90,8 +92,8 @@ If WP-Tracking is not used, the Plane will head straight to the landiung locatio * `nav_fw_pitch2thr`: The navigation throttle modifier has to be tuned well to allow stable navigation during climbs and descents to prevent a stall. Make sure your plane maintains Ground or Airspeed, when climbing in any navigation mode. The Craft should not get slower and not speed ub significantly during a navigation climb, if P2T is tuned properly. See `Fixed Wing Pitch To Throttle Tuning.md` for a full tuning procedure. -* `nav_wp_radius`: This parameter might be too high if you have set up your craft with INAV 6 or INAV 7. With a too high value, the turning points for the Crosswind-Leg and Final Approach are hit too early and make it difficult for the plane to align to the runway or cut short the approach. -Make sure this parameter is not set greater than 1000 (cm). The better your craft and navigation system is tuned, the lower this value can be. We recommend to start with 1000 for flying wings and 800 for a Plane with Tail. +* `nav_wp_radius`: The turning points for the Crosswind-Leg and Final Approach are calculated automatically from the aircraft's coordinated turn radius; `nav_wp_radius` does not shape the approach corners. +Make sure this parameter is not set greater than 1000 (cm) so approach waypoints are not detected as reached too early. * Test your Navigation-Tuning: A better Navigation-Tune will reward you with smoother and more reliable landings. To test your nav systems limit, we recommend to create a waypoint missions with many 90° turn angles with shorter and shorter tracks. With this Method, you can find out how well your plane can follow a navigation path and how long it takes to align to a waypoint track. A well tuned plane should be able to pull of a WP Mission that looks like this, where the distance between WP6 and WP7 si recommended to be the minimum approach length: diff --git a/docs/Settings.md b/docs/Settings.md index 5c0efd79be4..5a9c93c6d3b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4201,7 +4201,7 @@ COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. Th ### nav_fw_wp_turn_mode -How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn (fallback for users who prefer the old behaviour). The COORD modes command an explicit coordinated arc of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY anticipates the turn so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). COORD_FLYOVER flies over the waypoint, then rolls out exactly on the tangent line to the next waypoint - or, with nav_fw_wp_tracking_accuracy enabled, an S-turn that rolls out directly ON the new leg. COORD_FLYINTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries). +How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn. The COORD modes fly coordinated arcs of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY cuts the corner and passes the waypoint abeam, COORD_FLYOVER overflies the waypoint before turning onto the next leg, COORD_FLYINTO crosses the waypoint already aligned with the outbound leg (survey line entries). | Allowed Values | | | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0a2bd2445ae..a6f11c5ff2b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2671,7 +2671,7 @@ groups: min: 30 max: 80 - name: nav_fw_wp_turn_mode - description: "How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn (fallback for users who prefer the old behaviour). The COORD modes command an explicit coordinated arc of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY anticipates the turn so the arc joins the next leg without over/undershoot; the waypoint is passed abeam (corner cut). COORD_FLYOVER flies over the waypoint, then rolls out exactly on the tangent line to the next waypoint - or, with nav_fw_wp_tracking_accuracy enabled, an S-turn that rolls out directly ON the new leg. COORD_FLYINTO eases away from the corner before the waypoint, then flies an arc that crosses the waypoint already aligned on the outbound course (survey/mapping line entries)." + description: "How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn. The COORD modes fly coordinated arcs of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY cuts the corner and passes the waypoint abeam, COORD_FLYOVER overflies the waypoint before turning onto the next leg, COORD_FLYINTO crosses the waypoint already aligned with the outbound leg (survey line entries)." default_value: "COORD_FLYBY" field: fw.wp_turn_mode table: nav_fw_wp_turn_mode diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 188e977ab2c..f7b77daf1e5 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -676,8 +676,14 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) fwArcActive = false; + // Landing approach always flies coordinated FLY_BY turns, whatever mode is configured + navFwWpTurnMode_e turnMode = navConfig()->fw.wp_turn_mode; + if (posControl.navState == NAV_STATE_FW_LANDING_APPROACH) { + turnMode = NAV_FW_WP_TURN_COORD_FLY_BY; + } + const bool wpTracking = isWaypointNavTrackingActive() && !needToCalculateCircularLoiter; - if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_DIRECT || !wpTracking) { + if (turnMode == NAV_FW_WP_TURN_DIRECT || !wpTracking) { fwArcEngaged = false; fwArcPrevLegBearing = -1; intoStage = FW_INTO_IDLE; @@ -697,7 +703,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) intoStage = FW_INTO_IDLE; // a new leg invalidates any staged S geometry const bool capped = fwFlyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits fwFlyByCappedLatch = false; // consume the latch on any leg change - if (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_OVER) { + if (turnMode == NAV_FW_WP_TURN_COORD_FLY_OVER) { // FLY_OVER: circle pinned at the overfly point. Tracking OFF: exit on the tangent // through the next WP; tracking ON: bounded-intercept S onto the new leg itself. const float px = posControl.activeWaypoint.pos.x; @@ -784,7 +790,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } const int32_t hdgErr = wrap_18000(legBearing - cog); - if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_COORD_FLY_OVER && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + if (turnMode != NAV_FW_WP_TURN_COORD_FLY_OVER && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); @@ -833,12 +839,12 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } } } - if (!fwArcEngaged && navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_COORD_FLY_BY && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { + if (!fwArcEngaged && turnMode != NAV_FW_WP_TURN_COORD_FLY_BY && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { if (intoStage == FW_INTO_MAIN) { // crossing flown: re-arm once the leg has switched (normally it already has, mid-arc) intoStage = (ABS(wrap_18000(legBearing - intoBOut)) < 500) ? FW_INTO_IDLE : FW_INTO_DONE; } - if (intoStage == FW_INTO_IDLE && navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_INTO) { + if (intoStage == FW_INTO_IDLE && turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { const int32_t nta = posControl.activeWaypoint.nextTurnAngle; if (nta != -1 && ABS(nta) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { const float arcRtmp = getFwCoordinatedTurnRadius(); @@ -1083,7 +1089,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t * at any speed. Only runs when nextTurnAngle is set (FLY_BY waypoints + landing); FLY_OVER skips it. */ int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); posControl.flags.wpTurnSmoothingActive = false; - const bool flyIntoMissionLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_INTO && (navGetCurrentStateFlags() & NAV_AUTO_WP); + const bool flyIntoMissionLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_INTO && (navGetCurrentStateFlags() & NAV_AUTO_WP) + && posControl.navState != NAV_STATE_FW_LANDING_APPROACH; // landing approach keeps FLY_BY corner cuts if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && !flyIntoMissionLeg && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { const float turnRadius = getFwCoordinatedTurnRadius(); const float halfAngleTan = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 0.0f, NAV_FW_TURN_LEAD_TAN_MAX); From a5e5915fea080c2767e3f9ca056debdf4d4a54f0 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Sat, 22 Aug 2026 16:36:10 +0200 Subject: [PATCH 19/21] FW nav: loiter circle controller - steady arc law replaces the additive FF assist Additive FF on the carrot PID settles inside the commanded circle (HITL: 86-92 m flown at 120 m set) and its binary engage gate sat exactly in that error band, toggling the full circulation bank (POSHOLD bank flapping). Once established on the hold circle the steady arc law now takes over as a latch and regulates the radius directly; approach stays on stock guidance, loiter PID limit back to the planning bank. HITL: round circles at 30 km/h wind across a 65-145 km/h speed sweep, clean adaptive radius migration. Co-Authored-By: Claude Fable 5 --- docs/development/fw-turn-debugging.md | 6 +- src/main/navigation/navigation_fixedwing.c | 80 ++++++++++++++++------ 2 files changed, 62 insertions(+), 24 deletions(-) diff --git a/docs/development/fw-turn-debugging.md b/docs/development/fw-turn-debugging.md index a152eb76f79..49ec76eacd9 100644 --- a/docs/development/fw-turn-debugging.md +++ b/docs/development/fw-turn-debugging.md @@ -15,7 +15,7 @@ blackbox logging. | 2 | Exit course of the active arc | centideg | Arc coordinator, only while engaged | | 3 | Remaining heading to the exit course | centideg | Arc coordinator, only while engaged | | 4 | Arc bank command | centideg | Arc coordinator, only while engaged; clamped to the effective bank ceiling | -| 5 | Turn/loiter roll feed-forward | centideg | Feed-forward (0 when disabled, not established on the loiter circle, or inside the heading deadband) | +| 5 | WP-turn roll feed-forward | centideg | Feed-forward (0 when disabled, loitering, or inside the heading deadband) | | 6 | Energy-guard bank ceiling | deg | Energy bank guard (sits at `max_angle_inclination_rll` unless the guard is reducing it) | | 7 | Roll ease time | ms | Arc coordinator, only while engaged (sizes the entry/exit ramps and the turn-start lead) | @@ -25,7 +25,9 @@ whether the coordinator is active. ## Channel 1: coordinator state While the arc coordinator is engaged the value is `(arc phase + 1) * 10 + S stage`; while idle -it is the S stage alone. +it is the S stage alone. Two loiter states use their own values: `4` = holding, approach on the +stock carrot guidance; `40` = loiter circle controller engaged (steady arc law holds the +radius). Arc phase (tens digit): diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index f7b77daf1e5..09d3dfeb545 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -99,8 +99,9 @@ // Turn-coordination feed-forward: heading-error window over which the WP-turn FF tapers in (centideg) #define NAV_FW_FF_HEADING_DEADBAND_CD 500.0f // below this heading error: no WP-turn FF #define NAV_FW_FF_HEADING_FULL_CD 3000.0f // heading error for full WP-turn FF -#define NAV_FW_LOITER_FF_RADIAL_BAND 0.3f // fraction of R: loiter FF only within this band around the circle radius -#define NAV_FW_LOITER_FF_ALIGN_CD 6000 // [centideg] loiter FF only when cog is roughly tangential to the circle +#define NAV_FW_LOITER_CAPTURE_BAND 0.15f // fraction of R: engage the loiter circle controller inside this radial band +#define NAV_FW_LOITER_CAPTURE_ALIGN_CD 4500 // [centideg] and only roughly tangential to the circle +#define NAV_FW_LOITER_RELEASE_BAND 0.5f // fraction of R: grossly displaced -> hand back to the carrot guidance // If this is enabled navigation won't be applied if velocity is below 3 m/s //#define NAV_FW_LIMIT_MIN_FLY_VELOCITY @@ -116,7 +117,7 @@ static float fwRollSmoothSeedCd = 0.0f; // baseline the smoother re-seeds to 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 float fwEffectiveBankLimit = 0.0f; // adaptive nav bank limit (energy guard), deg; 0 = not yet initialised -static float fwActiveLoiterRadius = 0.0f; // effective loiter radius in use (cm), for the turn feed-forward +static float fwActiveLoiterRadius = 0.0f; // effective loiter radius in use (cm), for the loiter circle controller static bool fwArcActive = false; // arc turn coordinator is driving the turn (-> bank headroom, suppress cross-track, roll override) static bool fwArcEngaged = false; // arc coordinator latch across loops; must be cleared on controller reset or a stale arc resumes after a nav interruption static int32_t fwArcPrevLegBearing = -1; // last seen WP leg bearing [centideg] for leg-change detection (-1 = unseeded) @@ -454,10 +455,10 @@ static float getFwPlanningBankDeg(void) return MIN((float)navConfig()->fw.max_bank_angle, getFwEffectiveBankLimit()); } -// Roll-command bank limit [deg]: held loiter / active arc may use the reserve up to the ceiling to hold the radius against wind; everywhere else the target +// Roll-command bank limit [deg]: an active arc may use the reserve up to the ceiling to hold the radius against wind; everywhere else the target static float getFwControlBankLimit(void) { - return ((navGetCurrentStateFlags() & NAV_CTL_HOLD) || fwArcActive) ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); + return fwArcActive ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); } // Reduce the bank ceiling when a commanded climb stalls near the pitch/throttle limit while banked, so the turn widens and the climb recovers @@ -547,7 +548,7 @@ static float getFwCoordinatedTurnRadius(void) return constrainf(radius, NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX); } -// Coordinated-turn feed-forward bank [centideg] for the active loiter/WP turn (0 if disabled or straight). +// Coordinated-turn feed-forward bank [centideg] for the active WP turn (loiter has its own circle controller). static float getFwTurnFeedForward(int32_t navHeadingError) { const uint8_t ffGain = navConfig()->fw.turn_ff_gain; @@ -557,22 +558,7 @@ static float getFwTurnFeedForward(int32_t navHeadingError) float ffRadius = 0.0f; float ffSign = 0.0f; - if (needToCalculateCircularLoiter) { - // FF only once established on the circle - during the approach it fights the approach guidance - const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; - const float dcx = pos->x - posControl.desiredState.pos.x; - const float dcy = pos->y - posControl.desiredState.pos.y; - const float distToCenter = calc_length_pythagorean_2D(dcx, dcy); - const int8_t dir = loiterDirection(); - const float alpha = atan2_approx(dcy, dcx); - const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(dir * cos_approx(alpha), -dir * sin_approx(alpha))))); - const int32_t tangentErr = wrap_18000(tangentBearing - posControl.actualState.cog); - if (fabsf(distToCenter - fwActiveLoiterRadius) < NAV_FW_LOITER_FF_RADIAL_BAND * fwActiveLoiterRadius - && ABS(tangentErr) < NAV_FW_LOITER_FF_ALIGN_CD) { - ffRadius = fwActiveLoiterRadius; - ffSign = (float)dir; - } - } else if (isWaypointNavTrackingActive() && ABS(navHeadingError) > NAV_FW_FF_HEADING_DEADBAND_CD) { + if (!needToCalculateCircularLoiter && isWaypointNavTrackingActive() && ABS(navHeadingError) > NAV_FW_FF_HEADING_DEADBAND_CD) { ffRadius = getFwCoordinatedTurnRadius(); // WP turn: dynamic radius, tapered by heading error ffSign = (navHeadingError > 0 ? 1.0f : -1.0f) * constrainf((float)ABS(navHeadingError) / NAV_FW_FF_HEADING_FULL_CD, 0.0f, 1.0f); } @@ -1045,6 +1031,55 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) fwArcActive = true; } +// Loiter circle controller: once established on the hold circle, the steady arc law replaces the +// carrot PID - live FF bank plus radial/tangent feedback hold the stabilised radius exactly +static void updateFwLoiterArc(timeDelta_t deltaMicros) +{ + static bool established = false; + + if (!needToCalculateCircularLoiter || fwArcEngaged) { + established = false; + return; + } + + const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; + const float dcx = pos->x - posControl.desiredState.pos.x; + const float dcy = pos->y - posControl.desiredState.pos.y; + const float dist = calc_length_pythagorean_2D(dcx, dcy); + const float arcRadius = MAX(fwActiveLoiterRadius, (float)NAV_FW_TURN_RADIUS_MIN); + const int8_t dir = loiterDirection(); + const float alpha = atan2_approx(dcy, dcx); + const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(dir * cos_approx(alpha), -dir * sin_approx(alpha))))); + const int32_t eH = wrap_18000(tangentBearing - posControl.actualState.cog); + const float eR = dist - arcRadius; + + if (!established) { + if (fabsf(eR) < NAV_FW_LOITER_CAPTURE_BAND * arcRadius && ABS(eH) < NAV_FW_LOITER_CAPTURE_ALIGN_CD) { + established = true; + fwArcBankCmd = fwLastNavRollCmdCd; // blend from the current command: no engage step + } else { + DEBUG_SET(DEBUG_FW_TURN, 1, 4); // loiter approach, carrot guidance + return; + } + } else if (fabsf(eR) > NAV_FW_LOITER_RELEASE_BAND * arcRadius) { + established = false; // grossly displaced: hand back to the carrot + return; + } + + const float v = posControl.actualState.velXY; + const float phiLiveCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRadius))); + const float targetCd = dir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; + const float tEase = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiLiveCd)); + const float maxStepCd = phiLiveCd * (US2S(deltaMicros) * 1000.0f) / MAX(tEase, 1.0f); + fwArcBankCmd += constrainf(targetCd - fwArcBankCmd, -maxStepCd, maxStepCd); + const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); + fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); + + DEBUG_SET(DEBUG_FW_TURN, 1, 40); // loiter circle controller engaged + DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); + fwArcActive = true; +} + static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t deltaMicros) { if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) { @@ -1122,6 +1157,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t // Arc turn coordinator: manages the turn state and commands the roll bank directly updateFwTurnArc(deltaMicros); + updateFwLoiterArc(deltaMicros); // Calculate virtual waypoint virtualDesiredPosition.x = navGetCurrentActualPositionAndVelocity()->pos.x + posErrorX * (trackingDistance / distanceToActualTarget); From 7f0d7edc5d75f578fbe657da291236b045196a6f Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Sat, 22 Aug 2026 17:19:58 +0200 Subject: [PATCH 20/21] FW nav: settings wording - bank angle as sustained target, drop dev flags from turn tuning settings Co-Authored-By: Claude Fable 5 --- docs/Settings.md | 6 +++--- src/main/fc/settings.yaml | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 5a9c93c6d3b..fef6e2dd2b5 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3631,7 +3631,7 @@ P gain of auto speed PID controller. ### nav_fw_bank_angle -Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll +Maximum sustained roll angle when turning in GPS assisted modes: the target bank that turn and loiter radii are planned for. Corrections may exceed it temporarily; the absolute ceiling remains max_angle_inclination_rll | Default | Min | Max | | --- | --- | --- | @@ -4151,7 +4151,7 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within ### nav_fw_turn_ff_gain -DEVELOPER/EXPERIMENTAL (to be hardcoded before release): turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn/loiter radius forward to the roll controller so the PID only trims the residual, giving cleaner coordinated turns and more precise loiter circles. 0 disables the feed-forward (pure PID). +Turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn radius forward to the roll controller so the PID only trims the residual. 0 disables the feed-forward (pure PID). Default fits most models; tuning candidate to be fixed once field-proven. | Default | Min | Max | | --- | --- | --- | @@ -4181,7 +4181,7 @@ Sets the maximum allowed alignment convergence angle to the waypoint course line ### nav_fw_wp_turn_control_ease -DEVELOPER/EXPERIMENTAL: unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramp; SIM low, real models higher. +Unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramps; increase for large or slow-responding airframes. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a6f11c5ff2b..b5469a08bad 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2676,7 +2676,7 @@ groups: field: fw.wp_turn_mode table: nav_fw_wp_turn_mode - name: nav_fw_turn_ff_gain - description: "DEVELOPER/EXPERIMENTAL (to be hardcoded before release): turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn/loiter radius forward to the roll controller so the PID only trims the residual, giving cleaner coordinated turns and more precise loiter circles. 0 disables the feed-forward (pure PID)." + description: "Turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn radius forward to the roll controller so the PID only trims the residual. 0 disables the feed-forward (pure PID). Default fits most models; tuning candidate to be fixed once field-proven." default_value: 100 field: fw.turn_ff_gain min: 0 @@ -2688,7 +2688,7 @@ groups: min: 3000 max: 12000 - name: nav_fw_wp_turn_control_ease - description: "DEVELOPER/EXPERIMENTAL: unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramp; SIM low, real models higher." + description: "Unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramps; increase for large or slow-responding airframes." default_value: 100 field: fw.wp_turn_control_ease min: 0 @@ -2990,7 +2990,7 @@ groups: field: mc.slowDownForTurning type: bool - name: nav_fw_bank_angle - description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll" + description: "Maximum sustained roll angle when turning in GPS assisted modes: the target bank that turn and loiter radii are planned for. Corrections may exceed it temporarily; the absolute ceiling remains max_angle_inclination_rll" default_value: 35 field: fw.max_bank_angle min: 5 From 473588f8c746fe6cc5654a89e6202028936e276f Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Sat, 22 Aug 2026 17:47:21 +0200 Subject: [PATCH 21/21] FW nav: review fixes - DIRECT is a true legacy opt-out, collapse PG bump - corner-cut anticipation gated to FLY_BY legs (landing approach still forces it in every mode); DIRECT no longer advances waypoints early - turn feed-forward returns 0 in DIRECT outside the landing approach - PG_NAV_CONFIG collapsed to a single increment vs maintenance-10.x (8 -> 9) Co-Authored-By: Claude Fable 5 --- src/main/navigation/navigation.c | 2 +- src/main/navigation/navigation_fixedwing.c | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 3063ef33d9c..9b7a55339a0 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, 14); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 09d3dfeb545..732acdbbf99 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -552,8 +552,9 @@ static float getFwCoordinatedTurnRadius(void) static float getFwTurnFeedForward(int32_t navHeadingError) { const uint8_t ffGain = navConfig()->fw.turn_ff_gain; - if (ffGain == 0 || posControl.actualState.velXY <= NAV_FW_TURN_MIN_SPEED) { - return 0.0f; + if (ffGain == 0 || posControl.actualState.velXY <= NAV_FW_TURN_MIN_SPEED + || (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_DIRECT && posControl.navState != NAV_STATE_FW_LANDING_APPROACH)) { + return 0.0f; // DIRECT = pure legacy PID (landing approach forces FLY_BY) } float ffRadius = 0.0f; @@ -1121,12 +1122,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t } /* FLY_BY corner cut: start the turn R*tan(angle/2) before the WP so the arc joins the next leg - * at any speed. Only runs when nextTurnAngle is set (FLY_BY waypoints + landing); FLY_OVER skips it. */ + * at any speed. FLY_BY legs only - the landing approach forces FLY_BY in every mode. */ int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); posControl.flags.wpTurnSmoothingActive = false; - const bool flyIntoMissionLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_INTO && (navGetCurrentStateFlags() & NAV_AUTO_WP) - && posControl.navState != NAV_STATE_FW_LANDING_APPROACH; // landing approach keeps FLY_BY corner cuts - if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && !flyIntoMissionLeg && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { + const bool flyByLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_BY + || posControl.navState == NAV_STATE_FW_LANDING_APPROACH; + if (flyByLeg && waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { const float turnRadius = getFwCoordinatedTurnRadius(); const float halfAngleTan = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 0.0f, NAV_FW_TURN_LEAD_TAN_MAX); // Roll-in lead: the ramp is back-loaded and the ground track lags the bank (k calibrated in flight)