Enable Fixed Wing Auto Speed During Waypoint Mission

[breadoven]

Adds ability to use fixed wing Auto Speed mode with waypoint missions.

Usage:

1. Setup mission with waypoint speed defined for waypoint legs, i.e. set speed values for waypoint P1 parameter for Basic and Landing waypoint types or P2 parameter for Poshold waypoint type.
2. Run mission with the Auto Speed Aux switch OFF. Auto Speed will be enabled using ground speed as the speed reference for all waypoint legs with a non zero speed set.
3. Enabling Auto Speed via its Aux switch during a mission will revert Auto Speed back to normal use with speed set via the defined Auto Speed control (fw_auto_speed_channel) and the ability to switch between ground or airspeed as the speed reference.

Notes:
Auto Speed max and min speed settings, fw_auto_speed_max_speed and fw_auto_speed_min_speed, take precedence over the waypoint mission speed settings.
PR includes provision to allow use of airspeed as the speed reference during Nav but it's currently not implemented (todo).

Appears to work as expected from HITL testing.

[qodo-code-review]

PR Summary by Qodo

Enable fixed-wing Auto Speed from waypoint mission speeds

✨ Enhancement 🕐 40+ Minutes

AI Description

• Enable fixed-wing Auto Speed during WP missions using per-waypoint speed parameters.
• Apply Auto Speed throttle demand in the main navigation loop and expose its active state.
• Preserve AUX-switch Auto Speed behavior, including min/max constraints and speed-source logic.

Diagram

graph TD
  WP["Waypoint mission (WP mode)"] --> NAV["NAV state (AUTO_WP)"] --> GAS["getActiveSpeed()"] --> REQ{"Auto Speed requested?"} --> AS["applyAutoSpeedThrottleDemand()"] --> THR["Throttle output"] --> OSD["OSD Auto Speed"]
  RC["RC AutoSpeed switch"] --> REQ

Loading

High-Level Assessment

The following are alternative approaches to this PR:

1. Inject a virtual BOXAUTOSPEED when WP speed is set

• ➕ Reuses existing Auto Speed enable path with minimal new conditions
• ➕ Keeps demand/source selection centralized in existing BOXAUTOSPEED logic
• ➖ Couples navigation behavior to RC-mode semantics
• ➖ Harder to debug because the RC mode state no longer reflects pilot intent

2. Add an explicit NAV speed-control request flag + speed-source enum

• ➕ Clearer contract between nav state machine and Auto Speed (ground vs air)
• ➕ Easier to extend to true airspeed-by-nav later without special-casing
• ➖ Larger refactor across nav state/flags plumbing
• ➖ More review and regression surface area than this PR’s incremental approach

3. Validate/clamp waypoint speed at mission load/parsing time

• ➕ Keeps runtime control logic simpler and avoids enabling Auto Speed for tiny nonzero speeds
• ➕ Single place to enforce min/max and units
• ➖ Mission parsing code may be shared across vehicle types and require broader changes
• ➖ Less flexible if future features want dynamic clamping based on flight context

Recommendation: The PR’s approach (treating WP-mode + nonzero active speed as an Auto Speed request, while preserving AUX-switch semantics) is a pragmatic incremental change and aligns with the stated usage. Two follow-ups worth considering: (1) make the new autoSpeedIsActive symbol file-static to avoid exporting an unnecessary global, and (2) evolve isAutoSpeedRequiredByNav() to return an explicit speed-source (ground/air) once airspeed-by-nav is implemented, so the current placeholder logic doesn’t become entrenched.

Files changed (3) +59 / -26

Enhancement (3) +59 / -26

osd.cShow Auto Speed OSD element when Auto Speed is nav-activated +1/-1

Show Auto Speed OSD element when Auto Speed is nav-activated

• Updates OSD_AUTO_SPEED rendering to display whenever fixed-wing Auto Speed is active, not only when the AUX mode is enabled. This ensures mission-triggered Auto Speed still shows speed source and demand on the OSD.

src/main/io/osd.c

navigation.cDerive active speed from waypoint parameters for airplanes and call Auto Speed globally +22/-18

Derive active speed from waypoint parameters for airplanes and call Auto Speed globally

• Extends getActiveSpeed() to return per-waypoint speed parameters during NAV_AUTO_WP legs, including fixed-wing missions, while retaining multirotor manual-speed limiting behavior. Moves applyAutoSpeedThrottleDemand() to run after all vehicle navigation controllers, relying on internal gating for non-airplanes.

src/main/navigation/navigation.c

navigation_fixedwing.cEnable fixed-wing Auto Speed from WP mission speed and track active state +36/-7

Enable fixed-wing Auto Speed from WP mission speed and track active state

• Introduces a persistent Auto Speed active flag and a nav-based enable path that activates Auto Speed in WP mode when a waypoint speed is set. Updates demand calculation to use the AUX-controlled channel when the switch is on, otherwise using getActiveSpeed() and forcing ground-speed reference (with a stub path for future airspeed-by-nav).

src/main/navigation/navigation_fixedwing.c

[qodo-code-review]

Code Review by Qodo

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

1. Stale autospeed active flag 🐞 Bug ≡ Correctness

Description

isFixedwingAutoSpeedActive() now returns a cached flag that is only updated inside
applyAutoSpeedThrottleDemand(), but fixed-wing throttle logic checks isFixedwingAutoSpeedActive()
earlier in the same navigation loop. When Auto Speed becomes disabled, the fixed-wing controller can
incorrectly skip its normal throttle path for one cycle, leaving rcCommand[THROTTLE] without the
expected nav throttle control for that iteration.

Code

src/main/navigation/navigation_fixedwing.c[R890-893]

bool isFixedwingAutoSpeedActive(void)
+{
+    return autoSpeedIsActive;
+}

Evidence

The fixed-wing throttle controller checks isFixedwingAutoSpeedActive() while building
rcCommand[THROTTLE], but applyAutoSpeedThrottleDemand() (which sets/clears the cached flag) is only
called afterward in the main nav loop. Therefore, during transitions (notably disabling Auto Speed),
the controller can act on the previous loop’s Auto Speed state and skip the intended throttle update
for one iteration.

src/main/navigation/navigation_fixedwing.c[674-735]
src/main/navigation/navigation_fixedwing.c[887-923]
src/main/navigation/navigation.c[4403-4458]

Agent prompt

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

### Issue description
`isFixedwingAutoSpeedActive()` now returns a cached variable (`autoSpeedIsActive`) that is only set/cleared inside `applyAutoSpeedThrottleDemand()`. But the fixed-wing nav controller consults `isFixedwingAutoSpeedActive()` before `applyAutoSpeedThrottleDemand()` is called in the same control loop, so it can use stale state (especially on disable), skipping its normal throttle update for one loop.

### Issue Context
`applyFixedWingPitchRollThrottleController()` uses `isFixedwingAutoSpeedActive()` to decide whether to run the normal throttle correction path. `applyAutoSpeedThrottleDemand()` (which updates `autoSpeedIsActive`) is invoked after the fixed-wing controller from `applyWaypointNavigationAndAltitudeHold()`.

### Fix Focus Areas
- src/main/navigation/navigation_fixedwing.c[890-923]
- src/main/navigation/navigation_fixedwing.c[674-735]
- src/main/navigation/navigation.c[4447-4458]

### Proposed fix
Make `isFixedwingAutoSpeedActive()` reflect the *current* enablement conditions (e.g., `STATE(AIRPLANE) && isAutoSpeedEnabled()`), not a cached value updated later. If you still need a cached “last applied” flag for OSD/telemetry, keep it separate (e.g., `autoSpeedWasAppliedThisLoop`) and don’t use it for control-path gating.

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

---

2. Waypoint speed sign wrap ✓ Resolved 🐞 Bug ≡ Correctness

Description

getActiveSpeed() reads signed waypoint params (p1/p2 are int16_t) into a uint16_t and returns it
directly for AIRPLANE, so negative/sentinel values wrap to large positive speeds. In WP-enabled Auto
Speed, this wrapped value is then constrained, effectively commanding max Auto Speed unexpectedly.

Code

src/main/navigation/navigation.c[R4338-4347]

+        uint16_t wpSpecificSpeed = 0;
+        if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME) {
+            wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
+        } else {
+            wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
+        }

-            if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
-                waypointSpeed = wpSpecificSpeed;
-            } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
-                waypointSpeed = navConfig()->general.max_auto_speed;
-            }
+        if (STATE(AIRPLANE)) {
+            return wpSpecificSpeed;
+        } else if (wpSpecificSpeed >= 50) {  // min allowed speed of 0.5 m/s for multirotor

Evidence

Waypoint params are defined as signed int16_t, but getActiveSpeed() stores them in a uint16_t and
returns them for AIRPLANE without checking for negativity. The WP Auto Speed path then uses
constrain(getActiveSpeed(), minSpeed, maxSpeed), so wrapped negatives become maxSpeed.

src/main/navigation/navigation.h[553-560]
src/main/navigation/navigation.c[4321-4353]
src/main/navigation/navigation_fixedwing.c[939-948]

Agent prompt

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

### Issue description
`getActiveSpeed()` assigns `navWaypoint_t.p1/p2` (signed `int16_t`) to a `uint16_t` temporary and returns it for AIRPLANE without validating it. Negative values (e.g. `-1`) become large unsigned values (e.g. `65535`) and later get clamped to `maxSpeed`, producing an unintended full-speed demand.

### Issue Context
Waypoint params are stored in `navWaypoint_t` as signed integers. The fixed-wing WP Auto Speed path uses `getActiveSpeed()` to derive speed demand and constrains it between `fw_auto_speed_min_speed` and `fw_auto_speed_max_speed`.

### Fix Focus Areas
- src/main/navigation/navigation.c[4321-4353]
- src/main/navigation/navigation.h[553-560]
- src/main/navigation/navigation_fixedwing.c[939-948]

### Proposed fix
- Keep waypoint speed as a signed type (`int16_t`) when reading `p1/p2`.
- Treat `<= 0` as “no WP speed set” (return 0 or fall back to configured default).
- Optionally clamp to sane bounds before returning (e.g., `MIN(wpSpeed, navConfig()->general.max_auto_speed)` for MR; for FW, either clamp or rely on the later constrain but only after ensuring non-negative).

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

---

3. Autospeed flag has linkage ✓ Resolved 🐞 Bug ⚙ Maintainability

Description

autoSpeedIsActive is introduced as a non-static file-scope variable even though it is only used
within navigation_fixedwing.c. This unnecessarily exports a new global symbol and increases the risk
of future linkage collisions or accidental external modification.

Code

src/main/navigation/navigation_fixedwing.c[77]

+bool autoSpeedIsActive = false;

Evidence

The PR adds a new file-scope variable without static, creating an externally visible symbol. All
current reads/writes of this variable occur within the same source file.

src/main/navigation/navigation_fixedwing.c[69-78]

Agent prompt

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

### Issue description
`autoSpeedIsActive` is defined at file scope without `static`, giving it external linkage even though it is only referenced inside `navigation_fixedwing.c`.

### Issue Context
There is already a public accessor `isFixedwingAutoSpeedActive()`, so the backing flag does not need external linkage.

### Fix Focus Areas
- src/main/navigation/navigation_fixedwing.c[74-78]

### Proposed fix
Change the definition to `static bool autoSpeedIsActive = false;` (or move it into a private module struct) to keep the symbol local to the translation unit.

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

---

_{To customize comments, go to the Qodo configuration screen, or learn more in the docs.}

[qodo-code-review]

1. Stale autospeed active flag 🐞 Bug ≡ Correctness

isFixedwingAutoSpeedActive() now returns a cached flag that is only updated inside
applyAutoSpeedThrottleDemand(), but fixed-wing throttle logic checks isFixedwingAutoSpeedActive()
earlier in the same navigation loop. When Auto Speed becomes disabled, the fixed-wing controller can
incorrectly skip its normal throttle path for one cycle, leaving rcCommand[THROTTLE] without the
expected nav throttle control for that iteration.

Agent Prompt

### Issue description
`isFixedwingAutoSpeedActive()` now returns a cached variable (`autoSpeedIsActive`) that is only set/cleared inside `applyAutoSpeedThrottleDemand()`. But the fixed-wing nav controller consults `isFixedwingAutoSpeedActive()` before `applyAutoSpeedThrottleDemand()` is called in the same control loop, so it can use stale state (especially on disable), skipping its normal throttle update for one loop.

### Issue Context
`applyFixedWingPitchRollThrottleController()` uses `isFixedwingAutoSpeedActive()` to decide whether to run the normal throttle correction path. `applyAutoSpeedThrottleDemand()` (which updates `autoSpeedIsActive`) is invoked after the fixed-wing controller from `applyWaypointNavigationAndAltitudeHold()`.

### Fix Focus Areas
- src/main/navigation/navigation_fixedwing.c[890-923]
- src/main/navigation/navigation_fixedwing.c[674-735]
- src/main/navigation/navigation.c[4447-4458]

### Proposed fix
Make `isFixedwingAutoSpeedActive()` reflect the *current* enablement conditions (e.g., `STATE(AIRPLANE) && isAutoSpeedEnabled()`), not a cached value updated later. If you still need a cached “last applied” flag for OSD/telemetry, keep it separate (e.g., `autoSpeedWasAppliedThisLoop`) and don’t use it for control-path gating.

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

[github-actions]

Test firmware build ready — commit 0cd74ce

Download firmware for PR #11757

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