Fixed wing: flash/RAM pass on the coordinated turn coordinator (behaviour-identical refactor)

[b14ckyy]

Follow-up to #11812 after the flash/RAM feedback. Five behaviour-identical refactoring commits on the turn coordinator, one measurable step each. HITL before/after replay done (same mission, MATEKF765, debug_mode FW_TURN): per corner the arc phase sequence and its durations, engage distance, radius, exit course and ease time are identical between the pre-branch build and this head (see "HITL comparison" below). The last commit is a small behaviour fix found during that replay, kept separate.

Measured cost of #11812 (MATEKF722, -Os, LTO, commit before the merge vs the merge)

+8044 B flash, +248 B RAM. updateFwTurnArc alone was 4832 B (isolated with NOINLINE).

What this PR does

Commit	Step	F722 flash
047faca	Shared turn-geometry primitives in navigation_fixedwing_turn_math.c (bank-from-radius, bearing/unit vector, circle centre, line intersection, smoothstep, slew) + a unit test that memcmps each helper against the verbatim original expression	−384 B
9372af5	Per-tick values computed once; debug release, bank clamp and the landing-approach turn-mode rule single-sourced; the coordinator's debug channels written under one mode test	−192 B
26c6470	One function each for the five ramp-in engages, three capture fallbacks and two S stagings; the steady arc law shared by the WP arc and the loiter circle	−256 B
5cbf6e7	Whole predictor state in fwTurnState_t (148 B) + fwBankGuardState_t (64 B), zero-initialised, one reset function with exactly the previous reset set (per docs/development/ram-and-flash-optimization.md)	+52 B
e4be4b1	updateFwTurnArc split into per-mode planners, the two S-sequencer halves, leg-change retarget, leg-line capture, roll-out lead and the three phase steps, fed by one per-tick context: 351 lines / brace depth 8 → 90 lines / depth 3	0

Totals at the head: MATEKF722 −780 B flash / −8 B RAM, MATEKF405 −12 B / −24 B, MATEKH743 −1100 B / 0.

Evidence that behaviour is unchanged

• Step 1: 13 unit tests compare every helper bit-exactly with a verbatim copy of the replaced inline code over speed/radius/bearing sweeps; a negative control with the wrong unit macro fails as intended. Unit and firmware builds share the same FAST_MATH trig.
• Steps 3–5: a normalised reverse diff of each extracted or renamed block is byte-identical to the previous revision. The only control-flow edits are inverted early returns on integer guards; no store is reordered relative to a read.
• make check 573/573 after every step; zero warnings on MATEKF722, MATEKF405, MATEKH743 and SITL.
• Residual: the targets build with -ffp-contract=fast, so FMA fuse decisions after moved inlining boundaries cannot be proven on the host. The HITL replay covers this.

Notes

• The turn-mode rule is single-sourced at two of three sites; at the FLY_BY trigger the helper call costs 448 B on F405 (-O2), so that site keeps the inline form with a one-line note.
• The reset function deliberately does not memset the arc block: arc.bankCmd and arc.easeMs are read after a controller reset by a still-established loiter circle. S stage, loiter ratchet and the guard's filters were never part of the reset and still are not.
• No settings, PG or MSP changes.

Honest bottom line

The state-scope and nesting points are addressed. The bit-exact refactor recovers about 10 % of the flash: at -Os + LTO GCC was already folding most of the duplication, and the remaining ~7 KB is the geometry itself (trig calls of four turn modes). Going further means a per-target build flag that drops modes, or behaviour changes, which I would rather discuss than decide here.

HITL comparison (LOG00051 pre-branch vs LOG00052 this head, COORD_FLYINTO + tracking 5 m)

corner	build	engage dist	R	exit course	phase sequence (state, s)
WP3 (67°)	before	1011 m	33 m	125°	12 1.0 · 22 1.5 · 32 1.1 · 0 48.6 · 11 1.0 · 31 1.5
	after	1010 m	33 m	125°	12 1.0 · 22 1.5 · 32 1.1 · 0 48.5 · 11 1.0 · 31 1.5
WP4 (86°)	before	1296 m	33 m	58°	32 0.5 · 0 65.8 · 11 1.0 · 21 0.6 · 31 1.2
	after	1295 m	33 m	58°	32 0.5 · 0 65.8 · 11 1.0 · 21 0.6 · 31 1.2
WP5 (120°)	before	1095 m	31 m	332°	12 1.0 · 22 2.9 · 32 0.9 · 0 54.4 · 11 1.0 · 21 1.2 · 31 1.0
	after	1096 m	31 m	332°	12 1.0 · 22 3.0 · 32 0.9 · 0 54.3 · 11 1.0 · 21 1.2 · 31 1.0

Max bank command, actual roll and the mean command-vs-attitude error match to within 0.5° per corner. Residual differences are the flight's own noise (separate HITL runs).

Bug found during the replay (last commit, f85f5ff4a)

The FLY_INTO pickup marks the WP reached by setting wpTurnSmoothingActive once, but calculateVirtualPositionTarget_FW cleared that flag at the start of every position tick and the consumer (isWaypointReached, run from the nav FSM at RX cadence) missed the one-tick window at random. At corners where no geometric reached-check can fire (151° at WP6 in this mission) the aircraft then flew a full circle back to the passed waypoint. Seen in both builds, on different waypoints, so it is a race and not a refactor effect. The flag is now an event: set once, cleared only when consumed, on waypoint activation, and on any nav state change so it cannot leak into RTH or landing. FLY_BY is unaffected (it re-asserted the flag every tick anyway). This commit is the only behaviour change in the PR.

🤖 Generated with Claude Code

[github-actions]

RAM / Flash usage vs. base commit cd0108b — commit a5bd0b9

Using the nearest available size baseline — the PR's exact base commit has no stored baseline yet.

Target	Flash Δ	RAM Δ
MATEKF405	+260 B (+0.04%)	CCM: ±0 B (±0.00%) RAM: +12 B (+0.01%)
MATEKF722	-348 B (-0.07%)	ITCM_RAM: ±0 B (±0.00%) RAM: +8 B (+0.01%) TCM: ±0 B (±0.00%)
MATEKF765	+132 B (+0.02%)	DTCM_RAM: +136 B (+0.48%) SRAM1: +84 B (+0.07%)
MATEKH743	+396 B (+0.05%)	D2_RAM: ±0 B (±0.00%) DTCM_RAM: +116 B (+0.90%) ITCM_RAM: -184 B (-1.12%) RAM: +116 B (+0.08%)

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

[github-actions]

Test firmware build ready — commit a5bd0b9

Download firmware for PR #11996

249 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]

@sensei-hacker something seems to be off with the flash/ram check. The numbers don't match at all compared to my local builds. And apparently also not to your local builds since your 8k match exactly the baseline. and compared to baseline my results are the ones in this post for the refactor. But the flash check shows entirely different.

[sensei-hacker]

We can compare and contrast #11998

[b14ckyy]

Need to do some fixes first. It was not a perfect refactor and on the way I found a race condition causing WP not to hit. Caused by an old bug from remaining and reused turn smoothing code. When all is finished and validated I will compare again.

[qodo-code-review]

ⓘ 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]

PR Summary by Qodo

Optimize fixed-wing turn coordinator and fix waypoint event race

✨ Enhancement 🐞 Bug fix 🧪 Tests 🕐 40+ Minutes

AI Description

• Extracts shared geometry and decomposes turn coordination, reducing target flash and RAM.
• Preserves turn behavior with bit-exact helper tests and HITL replay validation.
• Persists FLY_INTO waypoint events until consumption, preventing missed mission transitions.

Diagram

graph TD
    A["Navigation tick"] --> B["Turn coordinator"] --> D{"Turn mode"}
    B --> C["Turn math"]
    D --> E["Arc phases"] --> G["Roll control"]
    D --> F["Loiter law"] --> G
    E --> H["Waypoint FSM"]

Loading

High-Level Assessment

The following are alternative approaches to this PR:

1. Keep geometry inline

• ➕ Minimizes structural churn in the flight-control path
• ➕ Avoids cross-translation-unit call boundaries
• ➖ Retains duplicated expressions and higher flash usage
• ➖ Leaves the coordinator monolithic and harder to review or maintain

2. Extract the entire coordinator module

• ➕ Creates a stronger boundary around state, planning, and phase execution
• ➕ Could simplify navigation_fixedwing.c further
• ➖ Introduces broader interface and lifecycle changes
• ➖ May increase flash through call boundaries and creates greater behavior-preservation risk

Recommendation: Retain the PR's staged approach: extract only reusable geometry, consolidate state, and split the coordinator into focused static planners and phase handlers. It achieves measurable memory savings and improved maintainability while limiting linkage changes; a full module extraction should be considered separately only after target-specific size measurements.

Files changed (7) +1263 / -606

Bug fix (1) +5 / -6

navigation.cPersist turn-generated waypoint events until consumption +5/-6

Persist turn-generated waypoint events until consumption

• Stops clearing the waypoint-turn event every position tick and instead clears it when consumed, when activating a new navigation state, or through normal waypoint lifecycle handling. This prevents the navigation FSM from randomly missing one-tick FLY_INTO pickup events.

src/main/navigation/navigation.c

Refactor (3) +879 / -600

navigation_fixedwing.cDecompose and consolidate the coordinated-turn controller +725/-600

Decompose and consolidate the coordinated-turn controller

• Moves predictor and bank-guard state into explicit structures, hoists per-tick inputs, and splits turn planning, S-sequencing, capture, phase execution, debug handling, and reset behavior into focused helpers. Reuses shared geometry and a common steady-arc law for waypoint turns and loiter while preserving control behavior and reducing flash/RAM.

src/main/navigation/navigation_fixedwing.c

navigation_fixedwing_turn_math.cAdd shared fixed-wing turn geometry primitives +115/-0

Add shared fixed-wing turn geometry primitives

• Implements reusable bank, bearing, tangent, offset, intersection, smoothstep, slew, cross-track, and roll-lead calculations. Selected helpers are kept out-of-line where measurements show lower flash usage.

src/main/navigation/navigation_fixedwing_turn_math.c

navigation_fixedwing_turn_math.hDeclare the shared turn-math contract +39/-0

Declare the shared turn-math contract

• Exposes the fixed-wing geometry and transition helpers while documenting that floating-point operand order and unit conversions are behavior-sensitive contracts.

src/main/navigation/navigation_fixedwing_turn_math.h

Tests (2) +377 / -0

CMakeLists.txtRegister fixed-wing turn-math unit dependencies +3/-0

Register fixed-wing turn-math unit dependencies

• Configures the unit-test target to compile the new turn-math and common math implementations.

src/test/unit/CMakeLists.txt

navigation_fixedwing_turn_math_unittest.ccVerify extracted turn math bit-for-bit +374/-0

Verify extracted turn math bit-for-bit

• Adds exhaustive sweeps comparing every extracted helper against verbatim legacy expressions, including wrapping, signed offsets, direction variants, and degenerate line intersections. Float comparisons use raw bits to detect rounding, signed-zero, or expression-order changes.

src/test/unit/navigation_fixedwing_turn_math_unittest.cc

Other (1) +2 / -0

CMakeLists.txtBuild shared fixed-wing turn math sources +2/-0

Build shared fixed-wing turn math sources

• Adds the new turn-math implementation and header to the common firmware source set.

src/main/CMakeLists.txt

[b14ckyy]

Validated in HITL so far. not more than 0.5m course deviation (no wind) in deterministic comparison of different turn modes with tracking on and off. behavior unchanged, debug shows no deviation in trigger and process values except that found bug that was uncovered and fixed in f85f5ff

[qodo-free-for-open-source-projects]

Code Review by Qodo

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

1. Invalid turn geometry corrupts arc plans ✓ Resolved 🐞 Bug ☼ Reliability

Description

fwLineIntersect uses an inverted  minAbsCross guard rejected it. When non-finite position or
direction data reaches the tracking-S or corner planner, it writes NaN coordinates and reports an
intersection, so those planners engage an invalid arc instead of their fallback.

Code

src/main/navigation/navigation_fixedwing_turn_math.c[R76-80]

+    if (fabsf(cross) <= minAbsCross) {
+        return false;
+    }
+
+    const float tt = ((p2x - p1x) * d2y - (p2y - p1y) * d2x) / cross;

Evidence

The newly added reference implementation records the replaced guard as a positive `fabsf(cross) >
minAbsCross` test, which returns false without writing outputs for NaN. IEEE comparisons with NaN
make both > and <= false, so the new inverted form falls through into the division and output
stores; the new planners treat a true result as usable geometry.

src/test/unit/navigation_fixedwing_turn_math_unittest.cc[109-122]
src/main/navigation/navigation_fixedwing_turn_math.c[74-84]
src/main/navigation/navigation_fixedwing.c[870-880]
src/main/navigation/navigation_fixedwing.c[956-966]

Agent prompt

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

## Issue description
`fwLineIntersect()` no longer preserves the old guard's behavior for NaN cross products: `fabsf(cross) <= minAbsCross` is false for NaN, so the helper calculates and writes NaN outputs and returns `true`.
## Fix Focus Areas
- src/main/navigation/navigation_fixedwing_turn_math.c[74-80]
## Recommended Fix
Use the original positive acceptance condition: only calculate and write the intersection when `fabsf(cross) > minAbsCross`; otherwise return `false`. This retains the previous fallback behavior for both near-parallel and non-finite geometry.

ⓘ 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 add REVIEW.md to your repo root and Qodo follows it on every PR

More tips ↗ | Customize Qodo ↗ | Qodo docs ↗