From 83cd2bbe7ebb517b33b59095297d5450e599272a Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 20 Sep 2026 23:29:05 -0500 Subject: [PATCH 1/3] Gate the FW arc turn coordinator behind USE_FW_TURN_PREDICTOR on <=512KB flash PR #11812's coordinated-turn arc coordinator (updateFwTurnArc, ~440 lines) and its direct helpers (getFwCoordinatedTurnRadius, getFwTurnFeedForward) account for ~8KB of the feature's flash footprint, concentrated almost entirely in one LTO-merged function. This alone is what pushed ZEEZF7V3 (STM32F722, 512KB flash) over its FLASH1 budget on later maintenance-10.x commits (PR #11924's CI run reported a 171-byte overflow there). wp_turn_mode's DIRECT setting (legacy heading-PID turn) already has a no-op path through the arc coordinator, so gating this code out and falling back to DIRECT behavior on <=512KB targets requires reviving nothing. USE_FW_TURN_PREDICTOR is added to common.h's existing "MCU_FLASH_SIZE > 512" gate list, alongside USE_AUTO_TRANSITION etc. Deliberately NOT gated: updateFwEnergyBankGuard, updateFwLoiterArc/ getFwStableLoiterRadius, and applyFwRollInSmoothing. These are general fixed-wing improvements independent of wp_turn_mode and apply to every flash size. Verified: ZEEZF7V3 FLASH1 490,743 B -> 484,703 B (-6,040 B). MATEKH743 (2048KB) keeps the feature compiled in (confirmed via preprocessor replay, since LTO inlines the gated symbols away either way). No conflict markers, no accidental deletions - a pure #ifdef wrap. --- src/main/navigation/navigation_fixedwing.c | 32 ++++++++++++++++++---- src/main/target/common.h | 1 + 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 482f19b6950..14aade54962 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -125,19 +125,24 @@ 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 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) -static bool fwFlyByCappedLatch = false; // the pending FLY_BY turn hit the lead-time cap -> fly it direct, not as an arc +static bool fwArcEngaged = false; // arc coordinator latch across loops; also read by the loiter arc to avoid fighting an active WP-turn arc static float fwArcBankCmd = 0.0f; // direct-radius arc bank command [centideg] (Approach B), applied to roll while fwArcActive static float fwArcHandbackCmdCd = 0.0f; // arc command at release; the PID/FF command is faded in from it so the static float fwArcHandbackMs = 0.0f; // seam is continuous regardless of nav_fw_control_smoothness (0 = no fade) static float fwArcHandbackDurMs = 0.0f; static float fwArcEaseMs = 0.0f; // ease time of the arc in progress, sizes the handback fade static bool fwArcWasActive = false; // arc drove the roll last frame, to catch the release edge +#ifdef USE_FW_TURN_PREDICTOR +// WP-turn arc coordinator + turn feed-forward state. Gated out on <=512KB flash targets, which fly +// wp_turn_mode as if it were DIRECT (the arc coordinator's own no-op path for that mode) - see +// updateFwTurnArc()/getFwTurnFeedForward() below. +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 float fwTurnFFCmdCd = 0.0f; // slew-limited turn feed-forward command [centideg] static bool fwTurnFFArmed = false; // FF assists the turn a leg change begins, not later tracking corrections static int32_t fwTurnFFPrevLegBearing = -1; // last leg bearing seen by the FF arming logic (-1 = unseeded) static float fwArcPickupAlong = 0.0f; // along-track distance to the second-arc pickup [cm], for the log +#endif static int8_t loiterDirYaw = 1; static bool needToCalculateCircularLoiter; static bool autoSpeedIsActive = false; @@ -346,11 +351,13 @@ void resetFixedWingPositionController(void) fwArcEngaged = false; fwArcWasActive = false; fwArcHandbackDurMs = 0.0f; +#ifdef USE_FW_TURN_PREDICTOR fwTurnFFCmdCd = 0.0f; fwTurnFFArmed = false; fwTurnFFPrevLegBearing = -1; fwArcPrevLegBearing = -1; fwFlyByCappedLatch = false; +#endif navPidReset(&posControl.pids.fw_nav); navPidReset(&posControl.pids.fw_heading); @@ -564,6 +571,7 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(fwEffectiveBankLimit)); // energy-guard bank ceiling [deg] } +#ifdef USE_FW_TURN_PREDICTOR // Coordinated-turn radius R = V^2/(g*tan(phi)) [cm], clamped. Times the FLY_BY turn for any speed. static float getFwCoordinatedTurnRadius(void) { @@ -620,6 +628,7 @@ static float getFwTurnFeedForward(int32_t navHeadingError, timeDelta_t deltaMicr DEBUG_SET(DEBUG_FW_TURN, 5, lrintf(fwTurnFFCmdCd)); // turn/loiter roll feed-forward [centideg] return fwTurnFFCmdCd; } +#endif // USE_FW_TURN_PREDICTOR // 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 @@ -703,6 +712,7 @@ static float applyFwArcHandbackFade(float rollTargetCd, timeDelta_t deltaMicros) return fwArcHandbackCmdCd + (rollTargetCd - fwArcHandbackCmdCd) * s; } +#ifdef USE_FW_TURN_PREDICTOR // 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) @@ -1146,6 +1156,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } fwArcActive = true; } +#endif // USE_FW_TURN_PREDICTOR // 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 @@ -1237,9 +1248,13 @@ 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. 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); + * at any speed. FLY_BY legs only - the landing approach forces FLY_BY in every mode. + * Gated out with the arc coordinator on <=512KB flash targets: without it to fly the corner cut, + * there is nothing to anticipate the turn for, so wpTurnSmoothingActive simply never engages and + * the leg behaves as a plain DIRECT turn at the waypoint. */ posControl.flags.wpTurnSmoothingActive = false; +#ifdef USE_FW_TURN_PREDICTOR + int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); 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) { @@ -1258,6 +1273,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t fwFlyByCappedLatch = turnCapped; // capped corner cut -> the arc coordinator flies it direct instead } } +#endif // USE_FW_TURN_PREDICTOR // We are closing in on a waypoint, calculate circular loiter if required if (needToCalculateCircularLoiter) { @@ -1272,7 +1288,9 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t } // Arc turn coordinator: manages the turn state and commands the roll bank directly +#ifdef USE_FW_TURN_PREDICTOR updateFwTurnArc(deltaMicros); +#endif updateFwLoiterArc(deltaMicros); // Calculate virtual waypoint @@ -1459,11 +1477,13 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta fwRollSmoothReseed = true; fwArcHandbackDurMs = 0.0f; // an arc that re-engages cancels a fade still in progress fwArcWasActive = true; +#ifdef USE_FW_TURN_PREDICTOR // The FF is not called while the arc drives: clear and disarm it, and consume leg changes the // arc handles itself - the carrot error left at hand-back is a capture correction, not a turn fwTurnFFCmdCd = 0.0f; fwTurnFFArmed = false; fwTurnFFPrevLegBearing = posControl.activeWaypoint.bearing; +#endif } else { if (fwArcWasActive) { // falling edge: arm the crossfade from the arc's last command fwArcHandbackCmdCd = fwLastNavRollCmdCd; @@ -1471,8 +1491,10 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta fwArcHandbackDurMs = fwArcEaseMs; fwArcWasActive = false; } +#ifdef USE_FW_TURN_PREDICTOR // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. rollAdjustment += getFwTurnFeedForward(navHeadingError, deltaMicros); +#endif rollAdjustment = applyFwArcHandbackFade(rollAdjustment, deltaMicros); rollAdjustment = applyFwRollInSmoothing(rollAdjustment, deltaMicros, fwRollSmoothReseed); fwRollSmoothReseed = false; diff --git a/src/main/target/common.h b/src/main/target/common.h index 4b23d187099..6beb532aa0e 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -440,6 +440,7 @@ // Keep larger optional features off 512 KB targets to preserve flash space. #if (MCU_FLASH_SIZE > 512) +#define USE_FW_TURN_PREDICTOR #define USE_AUTO_TRANSITION #define USE_TELEMETRY_MAVLINK #define USE_SERIALRX_MAVLINK From 115b5ff9da1089fa7e1a3bec9055d68e5ad06a44 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 21 Sep 2026 00:05:46 -0500 Subject: [PATCH 2/3] settings: mark two arc-coordinator settings condition:USE_FW_TURN_PREDICTOR nav_fw_turn_ff_gain and nav_fw_wp_turn_max_lead_time are only read by code gated behind USE_FW_TURN_PREDICTOR (getFwTurnFeedForward and the FLY_BY corner-cut block), so on <=512KB flash targets they were settable but silently inert. condition: hides them there, matching the existing USE_AUTO_TRANSITION-gated settings in this file. Also updated nav_fw_wp_turn_mode's description to note that its COORD_* values (COORD_FLYBY is the default) fall back to a plain DIRECT turn on those same targets, since the setting itself stays selectable - flagged by code review as worth surfacing since it affects the shipped default. docs/Settings.md regenerated via src/utils/update_cli_docs.py. --- docs/Settings.md | 6 +++--- src/main/fc/settings.yaml | 8 +++++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 5f609915d97..f31142acf63 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4260,7 +4260,7 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within ### nav_fw_turn_ff_gain -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. +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. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -4300,7 +4300,7 @@ Unmodelled roll-response lag (servo + airframe inertia) added to the computed ro ### nav_fw_wp_turn_max_lead_time -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. +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. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -4310,7 +4310,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. 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). +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). On targets with 512 KB flash or less, the arc coordinator is not built in to save flash: the COORD modes remain selectable but silently fly a plain DIRECT turn. | Allowed Values | | | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b071ce4d968..d4953df66ca 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2787,18 +2787,20 @@ 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. 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)." + 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). On targets with 512 KB flash or less, the arc coordinator is not built in to save flash: the COORD modes remain selectable but silently fly a plain DIRECT turn." default_value: "COORD_FLYBY" field: fw.wp_turn_mode table: nav_fw_wp_turn_mode - name: nav_fw_turn_ff_gain - 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." + 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. Available only on targets with more than 512 KB flash." + condition: USE_FW_TURN_PREDICTOR default_value: 100 field: fw.turn_ff_gain min: 0 max: 200 - name: nav_fw_wp_turn_max_lead_time - 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." + 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. Available only on targets with more than 512 KB flash." + condition: USE_FW_TURN_PREDICTOR default_value: 6000 field: fw.wp_turn_max_lead_time min: 3000 From d2287d73aa2aad19036cdf4bd0d40a6645247274 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 21 Sep 2026 00:14:38 -0500 Subject: [PATCH 3/3] navigation: guard turn_ff_gain/wp_turn_max_lead_time defaults with USE_FW_TURN_PREDICTOR The previous commit added condition: USE_FW_TURN_PREDICTOR to these two settings.yaml entries, so their SETTING_*_DEFAULT macros are no longer generated when the flag is undefined - breaking every <=512KB target with "undeclared" errors in navConfig_t's PG_RESET_TEMPLATE. Neither field is read anywhere outside the gated arc-coordinator code, so leaving them zero-initialized when gated is a safe substitute. Verified: ZEEZF7V3 links again (FLASH1 484,687 B, 98.61%), MATEKH743 unaffected (FLASH1 795,579 B, 43.36%, both settings still present). --- src/main/navigation/navigation.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 38f40895b47..ee2f247c285 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -283,8 +283,10 @@ 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, // COORD_FLYBY, WP mission turn mode +#ifdef USE_FW_TURN_PREDICTOR .turn_ff_gain = SETTING_NAV_FW_TURN_FF_GAIN_DEFAULT, // 100, turn FF .wp_turn_max_lead_time = SETTING_NAV_FW_WP_TURN_MAX_LEAD_TIME_DEFAULT, // 3000 ms +#endif .wp_turn_control_ease = SETTING_NAV_FW_WP_TURN_CONTROL_EASE_DEFAULT, // 100 ms } );