Draft — Fixed wing: predictive coordinated waypoint turns (Needs/Includes #11804) by b14ckyy · Pull Request #11812 · iNavFlight/inav

b14ckyy · 2026-08-22T15:31:57Z

Summary

FW waypoint turns are currently flown reactively: the heading PID chases a moving carrot, so
every corner is a late, speed-dependent overshoot followed by a correction back to track. This
PR plans the turn instead: from the aircraft's actual coordinated-turn radius
R = V²/(g·tan φ) (speed and nav_fw_bank_angle), each corner is constructed geometrically and
flown as an explicit coordinated arc — smoothstep roll-in, steady arc with live feed-forward plus
radial/tangent feedback, and a predictive roll-out that hands back aligned and level. Loiter
circles are held by the same steady law once established.

Detailed technical information:
coordinated-turns-overview.html

Companion PR: iNavFlight/inav-configurator#2711
Note: branch is stacked on fw-roll-smoothing (triggered S-curve roll smoothing + cruise
course lock). Either merge that first and rebase, or include it here — maintainer's call.

What it does

Turn modes (one consolidated setting, nav_fw_wp_turn_mode):

DIRECT — legacy heading-PID behaviour (opt-out).
COORD_FLYBY (default) — corner cut: the turn is anticipated by R·tan(Δ/2) plus a
roll-in lead, so the arc joins the next leg tangentially; the WP is passed abeam.
COORD_FLYOVER — the WP is overflown, then the aircraft rolls out exactly on the tangent
line to the next WP. With nav_fw_wp_tracking_accuracy enabled it instead flies a
bounded-intercept S (intercept ≤ 45° to the new leg, reverse-roll straight, corner-cut arc)
that rolls out ON the new leg.
COORD_FLYINTO — survey/mapping line entry: an opposite-sense ease-away arc plus a main
arc pinned at the WP cross the waypoint already aligned with the outbound course.

Robustness

Sharp corners (>160°) and lead-time-capped turns fly a bounded direct capture; with path
tracking enabled the capture steers onto the leg line (tapered intercept, capped at
nav_fw_wp_tracking_max_angle) instead of ending parallel to it a turn-diameter off.
Legs too short for the full manoeuvre degrade gracefully: consecutive turns merge into one
continuous capture; missions continue without error (HITL-verified).
Mission advance mid-arc retargets the capture onto the new leg; stale S geometry is released.
The autoland approach always flies COORD_FLYBY transitions, regardless of the configured mode.

Loiter

Once established on the hold circle (±15 % of R, roughly tangential), the steady arc law
replaces the carrot PID as a latch and regulates the radius directly (the pursuit carrot
plus any additive feed-forward always settles inside the commanded circle). Approach and
entry stay on stock guidance.
The commanded radius is floored by the physically flyable radius, stabilised with a
per-revolution peak hold (ratchet up instantly, decay slowly) — no more "kidney" deformation
in wind; the circle widens smoothly at speed and migrates back down afterwards.

Bank semantics: target vs. ceiling

nav_fw_bank_angle changes meaning: it is now the sustained target bank — the value all
turn and loiter radii are planned for — not an absolute limit for nav modes. Corrections
(holding an arc against wind, capturing the exit course) may exceed it temporarily.
max_angle_inclination_rll remains the absolute hard ceiling; no nav command ever
exceeds it. The reserve between target and ceiling is what makes the planned geometry
robust: the radius is sized for a bank the aircraft can always deliver, with headroom left
for corrections.

Energy bank guard (introduced to make that headroom safe)

Bank costs climb performance: at high bank the vertical lift component shrinks, and a
commanded climb near the pitch/throttle limit can stall while the reserve is being used.
The guard monitors climb-rate deficit against the commanded climb and the bank-induced sink.
When a banked climb stagnates, the effective bank ceiling eases down toward (and if needed
below) the planning target — the turn widens, the climb recovers. After a hold-off it
gradually releases back to the hard ceiling.
The effective ceiling feeds every consumer: arc bank commands, the loiter circle controller,
the PID clamp and the planning bank itself (a reduced ceiling also widens newly planned
radii). Manual throttle raises are respected as recovery input.
The arc coordinator only runs while WP tracking is active; RTH trackback and landing approach
use the FLY_BY construction.

Settings

Setting	Change
`nav_fw_wp_turn_mode`	Changed: DIRECT / COORD_FLYBY (default) / COORD_FLYOVER / COORD_FLYINTO
`nav_fw_bank_angle`	Meaning changed: sustained target bank (planning basis), temporarily exceedable for corrections; `max_angle_inclination_rll` stays the absolute ceiling
`nav_fw_wp_turn_max_lead_time`	New: cap on how early a COORD_FLYBY turn may start. 3000–12000 ms, default 6000
`nav_fw_turn_ff_gain`	New (CLI): turn feed-forward gain. 0–200 %, default 100 (0 = pure PID). Adjustable while the system proves itself in the field; hardcoding candidate later
`nav_fw_wp_turn_control_ease`	New (CLI): unmodelled roll-response lag added to the ease time. 0–500 ms, default 100. Same status: adjustable for now, hardcoding candidate later

Migration: PG_NAV_CONFIG is version-bumped — nav settings reset to defaults on first boot.

Debugging

debug_mode = FW_TURN exposes the full system (radius, coordinator state machine, exit course,
bank command, FF, guard ceiling, ease time) — documented in
docs/development/fw-turn-debugging.md. docs/Fixed Wing Landing.md updated (approach turning
points now derive from the coordinated turn radius).

Validation (HITL: X-Plane, MATEKF765, MSP debug telemetry)

FLY_BY: exits within centimetres–metres of the out-leg across 48–179° corners; residuals
correlate with the (uncompensated) wind vector estimated from the logs.
FLY_OVER tangent exit: hand-back within ±2° of the direct line at every corner incl. a full
reversal. Tracking-ON S: 14/14 corners at 0.5–6.5 m from the leg wind-off; worst 9.1 m at
≈15 km/h wind.
FLY_INTO: WP crossings 3–14 m across 47–92° corners.
Reversal fallback with path tracking: on the line within metres (previously 116 m parallel
offset and a late 46° tracker correction).
Loiter: round circles at 30 km/h wind across a 65–145 km/h speed sweep with clean adaptive
radius migration (previous additive-FF experiment settled 28 % inside the commanded radius —
replaced by the circle controller).
Mini-legs, autoland sequence (forced FLY_BY, headwind final): clean.
Geometry changes were pre-validated in a desktop kinematic simulator (controller logic + wind
- bank-lag plant) before HITL.

Known limitations / follow-ups

Ground-frame geometry: radius, anticipation and the steady law use ground speed; wind
shifts the effective radius around the turn (±3–9 m residuals at ~15 km/h wind).
Wind-vector compensation is planned as a follow-up PR (10.1).
Loiter entry/exit shaping (arc-follow approach) — later PR, builds on this module.
nav_fw_turn_ff_gain and nav_fw_wp_turn_control_ease stay CLI-adjustable until the system
is field-proven across airframes; both are candidates for hardcoding in a follow-up.

…y-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).

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.

…asing 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.

…fault 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 <noreply@anthropic.com>

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.

…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.)

…iter-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).

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).

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.

…ual 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).

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.

…edictive 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 <noreply@anthropic.com>

- 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 <noreply@anthropic.com>

…nt 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 <noreply@anthropic.com>

… 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 <noreply@anthropic.com>

…e 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 <noreply@anthropic.com>

…n 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 <noreply@anthropic.com>

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 <noreply@anthropic.com>

…ve 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 <noreply@anthropic.com>

…lags from turn tuning settings Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>

qodo-code-review · 2026-08-22T15:32:02Z

ⓘ Qodo reviews are paused because the subscription is no longer active. Ask your workspace admin to reactivate the subscription to resume reviews. Manage billing

qodo-free-for-open-source-projects · 2026-08-22T15:34:02Z

PR Summary by Qodo

Fixed-wing: predictive coordinated waypoint turns + loiter arc control

✨ Enhancement ⚙️ Configuration changes 📝 Documentation 🕐 40+ Minutes

AI Description

• Add coordinated-turn planning and an arc-based turn coordinator for FW waypoint missions.
• Replace continuous roll PT1 smoothing with triggered S-curve easing to avoid steady-state lag.
• Introduce new FW nav settings/debugging (turn modes, FF gain, lead-time cap, cruise lock gate).

Diagram

graph TD
  A["FW navigation loop"] --> B["FLY_BY lead planner"] --> C["Arc turn coordinator"] --> F["Roll command path"] --> G["RC roll output"]
  A --> D["Loiter radius stabilizer"] --> E["Loiter circle controller"] --> F
  A --> H["Energy bank guard"] --> F
  I["Settings/CLI/OSD"] --> A

High-Level Assessment

The following are alternative approaches to this PR:

1. Adopt an L1 / vector-field path follower (course+cross-track law)

➕ Single continuous controller for lines/circles; less explicit state machine
➕ Well-known behavior across many autopilots; simpler tuning surface
➖ Harder to guarantee exact tangent roll-out and waypoint-specific semantics (fly-by/over/into)
➖ Still needs special handling for very sharp corners and short legs
➖ Would require retuning against existing INAV FW dynamics and options

2. Keep carrot guidance but add only turn anticipation + feed-forward

➕ Smaller change set; lower risk than full arc coordinator
➕ Retains current PID-centric behavior and debugging mental model
➖ Still fundamentally reactive; overshoot/corner behavior remains speed-dependent
➖ Does not enable flyover/flyinto semantics or precise arc/radius holding

3. Spline/fillet mission preprocessing (path smoothing)

➕ Turns become inherently smooth; fewer runtime decisions
➕ Could merge consecutive short legs naturally
➖ Needs mission lookahead and reprocessing on mission edits/advances
➖ More complex failure modes when mission advances mid-turn; harder to debug in-flight

Recommendation: The PR’s explicit geometric arc coordinator is justified given the stated goals (predictive, radius-true coordinated turns with distinct fly-by/over/into semantics and robust short-leg behavior). If maintainers want a smaller first step, “anticipation + FF only” could be a staging approach, but it won’t deliver the same corner accuracy or mode semantics. If this is merged, prioritize follow-up wind compensation (ground-frame radius residuals are acknowledged) and consider eventually hardcoding/auto-tuning the temporary CLI gains once field data stabilizes.

Files changed (11) +1041 / -96 · 1 not counted

Enhancement (5) +894 / -75

debug.hAdd DEBUG_FW_TURN enum +1/-0

Add DEBUG_FW_TURN enum

• Registers a new debug type (DEBUG_FW_TURN) to expose coordinated-turn internal state via debug channels.

src/main/build/debug.h

cli.cAdd FW_TURN to debug_mode CLI names +2/-1

Add FW_TURN to debug_mode CLI names
• Extends the debugModeNames table to include FW_TURN, enabling selection via 'set debug_mode = FW_TURN'.
src/main/fc/cli.c

navigation.cAdd FW course-hold lock gating + bump NAV config PG version +42/-19

Add FW course-hold lock gating + bump NAV config PG version

• Adds optional FW COURSE HOLD behavior to delay course locking until the aircraft rolls out below ~10° bank (nav_cruise_lock_on_level). Bumps PG_NAV_CONFIG version (reset-on-first-boot migration) and ensures landing approach turn angles are always computed for coordinated FLY_BY transitions.

src/main/navigation/navigation.c

navigation.hDefine new FW WP turn mode enum and settings fields +10/-5

Define new FW WP turn mode enum and settings fields
• Replaces the wp_turn_smoothing enum with navFwWpTurnMode_e and extends navConfig_t with coordinated-turn parameters (mode, FF gain, lead-time cap, ease lag) plus cruise_lock_on_level.
src/main/navigation/navigation.h

navigation_fixedwing.cImplement predictive coordinated WP turns, loiter arc control, and energy bank guard +839/-50

Implement predictive coordinated WP turns, loiter arc control, and energy bank guard

• Replaces roll PT1 smoothing with a triggered smoothstep S-curve easing window to avoid steady tracking lag. Adds coordinated-turn radius calculation, FLY_BY anticipation using R·tan(Δ/2) with lead-time cap, an arc coordinator (ramp-in/steady/capture, plus S sequencing for flyover-tracking/flyinto), and a loiter circle controller that directly regulates radius once established. Introduces turn feed-forward, cross-track suppression and PID integrator freeze during arc flight, and an energy-aware effective bank ceiling that can reduce bank during banked climbs to preserve climb performance.

src/main/navigation/navigation_fixedwing.c

Bug fix (1)

pid.cFreeze level-trim PID integrator when trim conditions aren’t met _{not counted}

Freeze level-trim PID integrator when trim conditions aren’t met
• Adds PID_FREEZE_INTEGRATOR gating to the fixed-wing level-trim controller when trim is inactive or updates are too sparse, preventing integrator drift outside valid trim conditions.
src/main/flight/pid.c

Documentation (3) +112 / -9

Fixed Wing Landing.mdDocument coordinated FLY_BY turns in autoland approach +4/-2

Document coordinated FLY_BY turns in autoland approach

• Clarifies that landing approach leg transitions are always flown as coordinated FLY_BY corner cuts based on the real coordinated-turn radius. Updates guidance for nav_wp_radius: it no longer shapes approach corners, but should remain low to avoid early waypoint-reached detection.

docs/Fixed Wing Landing.md

Settings.mdAdd/rename FW nav settings and new FW_TURN debug mode docs +49/-7

Add/rename FW nav settings and new FW_TURN debug mode docs

• Documents the new debug_mode FW_TURN, new nav_cruise_lock_on_level behavior, revised nav_fw_bank_angle semantics, and updated nav_fw_control_smoothness roll S-curve description. Replaces the old FW WP turn smoothing setting with the new nav_fw_wp_turn_mode plus additional coordinated-turn tuning parameters (FF gain, ease lag, lead-time cap).

docs/Settings.md

fw-turn-debugging.mdAdd FW coordinated-turn debug channel reference +59/-0

Add FW coordinated-turn debug channel reference
• Introduces a developer-facing guide for DEBUG_FW_TURN, including channel meanings, coordinator state encoding, and practical interpretation of logs/telemetry during turns and loiter.
docs/development/fw-turn-debugging.md

Other (2) +35 / -12

cms_menu_navigation.cExpose WP turn mode in CMS/OSD menu +1/-1

Expose WP turn mode in CMS/OSD menu

• Replaces the legacy “WP TURN SMOOTHING” menu entry with “WP TURN MODE” so users can select DIRECT vs coordinated turn modes from the navigation CMS menu.

src/main/cms/cms_menu_navigation.c

settings.yamlReplace wp_turn_smoothing with wp_turn_mode and add new FW turn settings +34/-11

Replace wp_turn_smoothing with wp_turn_mode and add new FW turn settings

• Defines the new nav_fw_wp_turn_mode enum (DIRECT/COORD_FLYBY/COORD_FLYOVER/COORD_FLYINTO) and adds nav_fw_turn_ff_gain, nav_fw_wp_turn_max_lead_time, nav_fw_wp_turn_control_ease, and nav_cruise_lock_on_level. Updates setting descriptions to reflect new bank-angle semantics and roll S-curve smoothing.

src/main/fc/settings.yaml

qodo-free-for-open-source-projects · 2026-08-22T15:39:23Z

Code Review by Qodo

🐞 Bugs (1) 📘 Rule violations (0) 📎 Requirement gaps (0) 🎨 UX issues (0) 🔗 Cross-repo conflicts (0) 📜 Skill insights (0)

1. ~~DIRECT mode cuts corners~~ ✓ Resolved 🐞 Bug ≡ Correctness

Description

In DIRECT mode, the code can still arm the FLY_BY anticipation path and isWaypointReached() will
advance the mission as soon as the anticipated turn-start is reached, causing corner-cut/early
waypoint advance even though DIRECT is supposed to be legacy behavior.

Code

src/main/navigation/navigation_fixedwing.c[R1127-1130]

+    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();

Evidence

The mission precomputes nextTurnAngle even when the configured mode is DIRECT (only excluded for
FLY_OVER). That nextTurnAngle is then used to arm wpTurnSmoothingActive without checking for
COORD_FLYBY, and isWaypointReached() treats wpTurnSmoothingActive as an immediate
waypoint-reached condition—advancing early and cutting the corner in DIRECT.

src/main/navigation/navigation.c[4300-4313]
src/main/navigation/navigation_fixedwing.c[1123-1144]
src/main/navigation/navigation.c[3103-3115]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
DIRECT (legacy) waypoint turning is not respected: FLY_BY turn anticipation can still arm in DIRECT, and the mission can advance early (waypoint considered reached at turn start).
### Issue Context
- `calculateAndSetActiveWaypoint()` currently populates `activeWaypoint.nextTurnAngle` for all modes except COORD_FLYOVER.
- `calculateVirtualPositionTarget_FW()` uses `nextTurnAngle` to arm `wpTurnSmoothingActive` without checking that the configured turn mode is COORD_FLYBY (or landing-approach forced FLY_BY).
- `isWaypointReached()` returns `true` immediately when `wpTurnSmoothingActive` is set, which advances the mission early.
### Fix Focus Areas
- src/main/navigation/navigation.c[4300-4313]
- src/main/navigation/navigation_fixedwing.c[1123-1144]
- src/main/navigation/navigation.c[3103-3115]
### Suggested change
- Only compute `activeWaypoint.nextTurnAngle` for modes that actually need anticipation (COORD_FLYBY, COORD_FLYINTO, and landing approach override).
- Additionally (defense-in-depth), gate the FLY_BY corner-cut arming (`wpTurnSmoothingActive`) so it only runs when `wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_BY` (or landing approach state forces FLY_BY).
- Ensure DIRECT mode never sets `wpTurnSmoothingActive` and therefore never triggers early `isWaypointReached()`.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

2. ~~DIRECT mode gets turn feedforward~~ ✓ Resolved 🐞 Bug ≡ Correctness

Description

Even when nav_fw_wp_turn_mode is DIRECT, the roll controller still adds coordinated-turn
feed-forward (getFwTurnFeedForward()), so DIRECT cannot reproduce legacy heading-PID behavior.

Code

src/main/navigation/navigation_fixedwing.c[R1341-1344]

+    } 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);

Evidence
The roll feed-forward is added in the main FW nav roll path regardless of the selected turn mode,
and the feed-forward function itself only checks tracking state and heading error—not the configured
mode—so DIRECT mode behavior is modified.
src/main/navigation/navigation_fixedwing.c[1291-1347]
src/main/navigation/navigation_fixedwing.c[551-565]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
DIRECT mode is documented as legacy heading-PID behavior, but the new coordinated-turn feed-forward is applied unconditionally whenever WP tracking is active.
### Issue Context
- `updatePositionHeadingController_FW()` always executes `rollAdjustment += getFwTurnFeedForward(navHeadingError);` when not in an arc.
- `getFwTurnFeedForward()` does not check `navConfig()->fw.wp_turn_mode`; it only checks WP tracking/heading error/speed.
### Fix Focus Areas
- src/main/navigation/navigation_fixedwing.c[1341-1347]
- src/main/navigation/navigation_fixedwing.c[551-565]
### Suggested change
- Gate feed-forward by turn mode, e.g. return 0 (or skip calling) when `navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_DIRECT`.
- If you still want FF in DIRECT for some reason, update docs/setting semantics accordingly; otherwise DIRECT should be a true opt-out.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

3. Nav config resets on upgrade 🐞 Bug ☼ Reliability

Description

Bumping PG_NAV_CONFIG from version 8 to 14 causes *all* navigation settings to be reset to
defaults on upgrade because PG loading only restores stored values when the version matches exactly.

Code

src/main/navigation/navigation.c[124]
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14);

Evidence

navConfig is registered with version 14, and the PG loader only restores persisted values when
versions match; otherwise it keeps defaults after reset, which effectively resets the entire
navConfig on upgrade from older firmware.

src/main/navigation/navigation.c[124-127]
src/main/config/parameter_group.c[86-94]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
`navConfig` PG version change forces a full reset of the navigation configuration on firmware upgrade, wiping unrelated nav settings (not just the new FW turn fields).
### Issue Context
`pgLoad()` resets to defaults and only copies stored data when the stored version equals the registered version.
### Fix Focus Areas
- src/main/navigation/navigation.c[124-127]
- src/main/config/parameter_group.c[86-94]
- src/main/navigation/navigation.h[471-515]
### Suggested change options
1) **Avoid version bump** by keeping binary layout compatible (e.g., retain the old `wp_turn_smoothing` field as a deprecated/unused placeholder and append new fields at the end), so existing stored nav settings still load.
2) If a reset is truly required, consider splitting new settings into a separate PG (so only the new PG resets) or add explicit migration support (if available in this codebase).
(With the current PG implementation shown, any version mismatch discards stored navConfig values.)

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

Tip of the day

💡 Did you know, you can commit Qodo's fix in one click with committable suggestions (GitHub & GitLab)

More tips ↗ | Customize Qodo ↗ | Qodo docs ↗

- 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 <noreply@anthropic.com>

github-actions · 2026-08-22T15:50:39Z

RAM / Flash usage vs. base branch — commit 473588f

Target	Flash Δ	RAM Δ
MATEKF405 ⚠️	+10816 B (+1.64%)	+228 B (+0.16%)
MATEKF722 ⚠️	+7500 B (+1.62%)	+212 B (+0.17%)
MATEKF765 ⚠️	+11048 B (+1.61%)	+220 B (+0.15%)
MATEKH743 ⚠️	+10992 B (+1.53%)	+200 B (+0.13%)

See RAM/flash optimization guide for techniques to reduce usage.

github-actions · 2026-08-22T15:51:00Z

Test firmware build ready — commit 473588f

Download firmware for PR #11812

244 targets built. Find your board's .hex file by name on that page (e.g. MATEKF405SE.hex). Files are individually downloadable — no GitHub login required.

Development build for testing only. Use Full Chip Erase when flashing.

b14ckyy and others added 20 commits August 20, 2026 20:19

FW nav: settings wording - bank angle as sustained target, drop dev f…

7f0d7ed

…lags from turn tuning settings Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>

b14ckyy added this to the 10.0 milestone Aug 22, 2026

b14ckyy self-assigned this Aug 22, 2026

b14ckyy added Enhancement Testing Required Requires Configurator Documentation needed labels Aug 22, 2026

b14ckyy mentioned this pull request Aug 22, 2026

Advanced Tuning: FW coordinated WP turn settings - comes with FW #11812 iNavFlight/inav-configurator#2711

Open

qodo-free-for-open-source-projects Bot reviewed Aug 22, 2026

View reviewed changes

Comment thread src/main/navigation/navigation_fixedwing.c Outdated

Comment thread src/main/navigation/navigation_fixedwing.c

Comment thread src/main/navigation/navigation.c Outdated

breadoven mentioned this pull request Aug 23, 2026

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

Open

Navigation Menu

Uh oh!

Draft — Fixed wing: predictive coordinated waypoint turns (Needs/Includes #11804) - #11812

Draft — Fixed wing: predictive coordinated waypoint turns (Needs/Includes #11804) #11812
b14ckyy wants to merge 21 commits into
iNavFlight:maintenance-10.xfrom
b14ckyy:fw-turn-predictor

b14ckyy commented Aug 22, 2026 •

edited

Loading

Uh oh!

qodo-code-review Bot commented Aug 22, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Aug 22, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Aug 22, 2026 •

edited

Loading

Uh oh!

Uh oh!

Uh oh!

Uh oh!

github-actions Bot commented Aug 22, 2026 •

edited

Loading

Uh oh!

github-actions Bot commented Aug 22, 2026 •

edited

Loading

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

Uh oh!

Conversation

b14ckyy commented Aug 22, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

What it does

Settings

Debugging

Validation (HITL: X-Plane, MATEKF765, MSP debug telemetry)

Known limitations / follow-ups

Uh oh!

qodo-code-review Bot commented Aug 22, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Aug 22, 2026

PR Summary by Qodo

Uh oh!

qodo-free-for-open-source-projects Bot commented Aug 22, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Code Review by Qodo

Uh oh!

Uh oh!

Uh oh!

Uh oh!

github-actions Bot commented Aug 22, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

github-actions Bot commented Aug 22, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

b14ckyy commented Aug 22, 2026 •

edited

Loading

qodo-free-for-open-source-projects Bot commented Aug 22, 2026 •

edited

Loading

github-actions Bot commented Aug 22, 2026 •

edited

Loading

github-actions Bot commented Aug 22, 2026 •

edited

Loading