Skip to content

Enable Fixed Wing Auto Speed During Waypoint Mission #11757

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
breadoven wants to merge 3 commits into
base: maintenance-10.x
Choose a base branch
from
+65 −32
Open

Enable Fixed Wing Auto Speed During Waypoint Mission #11757

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
acfd1cd
enable fw autospeed from wp mode
breadoven Jul 30, 2026
0cd74ce
fixes
breadoven Aug 2, 2026
5470b9e
Update Navigation.md
breadoven Aug 3, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions docs/Navigation.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,8 @@ The navigation system in INAV is responsible for assisting the pilot allowing al

## NAV ALTHOLD mode - altitude hold

Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically.
In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - aircraft goes up, pilot moves stick down -
Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically.
In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - aircraft goes up, pilot moves stick down -
aircraft descends, you keep stick at neutral position - aircraft maintains current altitude.


Expand All @@ -31,7 +31,7 @@ When activated, this mode will attempt to keep copter where it is (based on GPS
### CLI parameters affecting POSHOLD mode:
* *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own.


### Related PIDs
PIDs affecting position hold: POS, POSR
PID meaning:
Expand Down Expand Up @@ -83,9 +83,9 @@ Parameters:

* `<alt>` - Altitude in cm. See `p3` bit 0 for datum definition.

* `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number). For SET_HEAD, it is the desired heading (0-359) or -1 to cancel a previous SET_HEAD or SET_POI.
* `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s). For multicopters it works for speeds > 0.5 m/s and < nav_auto_speed. The speed setting also applies for fixed wing from V10.0 where setting a speed activates fixed wing auto speed mode. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number). For SET_HEAD, it is the desired heading (0-359) or -1 to cancel a previous SET_HEAD or SET_POI.

* `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For JUMP it is the number of iterations of the JUMP.
* `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s). For multicopters it works for speeds > 0.5 m/s and < nav_auto_speed. The speed setting also applies for fixed wing from V10.0 where setting a speed activates fixed wing auto speed mode. For JUMP it is the number of iterations of the JUMP.

* `<p3>` - A bitfield with four bits reserved for user specified actions. It is anticipated that these actions will be exposed through the logic conditions.
* Bit 0 - Altitude (`alt`) : Relative (to home altitude) (0) or Absolute (AMSL) (1).
Expand Down
2 changes: 1 addition & 1 deletion src/main/io/osd.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1992,7 +1992,7 @@ static bool osdDrawSingleElement(uint8_t item)
break;

case OSD_AUTO_SPEED:
if (IS_RC_MODE_ACTIVE(BOXAUTOSPEED)) {
if (IS_RC_MODE_ACTIVE(BOXAUTOSPEED) || isFixedwingAutoSpeedActive()) {
buff[0] = posControl.autoSpeedSpdSource == FW_AUTO_SPD_GROUND ? 'G' : 'A';
strcpy(buff + 1, ": OFF");
if (isFixedwingAutoSpeedActive()) {
Expand Down
40 changes: 22 additions & 18 deletions src/main/navigation/navigation.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4320,28 +4320,32 @@ bool isNavHoldPositionActive(void)

float getActiveSpeed(void)
{
/* Currently only applicable for multicopter */
uint16_t waypointSpeed = 0;

// Speed limit for modes where speed manually controlled
if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
return navConfig()->general.max_manual_speed;
if (STATE(MULTIROTOR)) {
// Speed limit for modes where speed manually controlled
if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
return navConfig()->general.max_manual_speed;
}
waypointSpeed = navConfig()->general.auto_speed;
}

uint16_t waypointSpeed = navConfig()->general.auto_speed;
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && posControl.waypointCount > 0 &&
(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT ||
posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME ||
posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {

if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
float wpSpecificSpeed = 0.0f;
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
uint16_t wpSpecificSpeed = 0;
if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME) {
wpSpecificSpeed = ABS(posControl.waypointList[posControl.activeWaypointIndex].p2); // P1 is hold time
} else {
wpSpecificSpeed = ABS(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
Comment thread
qodo-code-review[bot] marked this conversation as resolved.
return MIN(wpSpecificSpeed, navConfig()->general.max_auto_speed);
}
}

Expand Down Expand Up @@ -4446,11 +4450,11 @@ void applyWaypointNavigationAndAltitudeHold(void)
applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
} else if (STATE(FIXED_WING_LEGACY)) {
applyFixedWingNavigationController(navStateFlags, currentTimeUs);
applyAutoSpeedThrottleDemand(&rcCommand[THROTTLE], currentTimeUs);
}
else {
applyMulticopterNavigationController(navStateFlags, currentTimeUs);
}
applyAutoSpeedThrottleDemand(&rcCommand[THROTTLE], currentTimeUs);

/* Consume position data */
if (posControl.flags.horizontalPositionDataConsumed)
Expand Down
45 changes: 37 additions & 8 deletions src/main/navigation/navigation_fixedwing.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,8 @@ static float throttleSpeedAdjustment = 0;
static bool isAutoThrottleManuallyIncreased = false;
static float navCrossTrackError;
static int8_t loiterDirYaw = 1;
bool needToCalculateCircularLoiter;
static bool needToCalculateCircularLoiter;
static bool autoSpeedIsActive = false;

// Calculates the cutoff frequency for smoothing out roll/pitch commands
// control_smoothness valid range from 0 to 9
Expand Down Expand Up @@ -887,18 +888,38 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
* Auto Speed mode control
*-----------------------------------------------------------*/
bool isFixedwingAutoSpeedActive(void)
{
return autoSpeedIsActive;
}
Comment on lines 890 to +893

@qodo-code-review qodo-code-review Bot Aug 1, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Action required

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


static int8_t isAutoSpeedRequiredByNav(void)
{
int8_t result = -1;
if (FLIGHT_MODE(NAV_WP_MODE) && getActiveSpeed() > 0) {
result = FW_AUTO_SPD_GROUND;
}

return result;
}

static bool isAutoSpeedEnabled(void)
{
bool thrStickEmergStop = navConfig()->fw.auto_speed_channel != (THROTTLE + 1) && throttleStickIsLow();
bool autoSpeedRequested = IS_RC_MODE_ACTIVE(BOXAUTOSPEED) || isAutoSpeedRequiredByNav() >= 0;

return STATE(AIRPLANE) && ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXAUTOSPEED) && isProbablyStillFlying() && !thrStickEmergStop &&
!FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
posControl.flags.estVelStatus == EST_TRUSTED && posControl.flags.estAltStatus == EST_TRUSTED &&
!(navigationRequiresAutoThrottleMode() && !(navGetCurrentStateFlags() & NAV_CTL_SPEED));
return ARMING_FLAG(ARMED) && autoSpeedRequested && isProbablyStillFlying() && !thrStickEmergStop &&
!FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
posControl.flags.estVelStatus == EST_TRUSTED && posControl.flags.estAltStatus == EST_TRUSTED &&
!(navigationRequiresAutoThrottleMode() && !(navGetCurrentStateFlags() & NAV_CTL_SPEED));
}

void applyAutoSpeedThrottleDemand(int16_t *throttleCommand, timeUs_t currentTimeUs)
{
if (!isFixedwingAutoSpeedActive()) return;
if (!STATE(AIRPLANE) || !isAutoSpeedEnabled()) {
autoSpeedIsActive = false;
return;
}
autoSpeedIsActive = true;

static uint16_t autoSpeedThrottleCommand = PWM_RANGE_MIDDLE;

Expand All @@ -918,16 +939,24 @@ void applyAutoSpeedThrottleDemand(int16_t *throttleCommand, timeUs_t currentTime
uint16_t minThrottle = MAX(getThrottleIdleValue(), currentBatteryProfile->nav.fw.min_throttle);
uint16_t maxThrottle = currentBatteryProfile->nav.fw.max_throttle;

posControl.desiredState.autoSpeedDemand = scaleRange(rxGetChannelValue(navConfig()->fw.auto_speed_channel - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, minSpeed, maxSpeed);
bool useAirSpeed = !LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_AUTOSPEED_AIRSPEED);
if (IS_RC_MODE_ACTIVE(BOXAUTOSPEED)) {
posControl.desiredState.autoSpeedDemand = scaleRange(rxGetChannelValue(navConfig()->fw.auto_speed_channel - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, minSpeed, maxSpeed);
} else {
posControl.desiredState.autoSpeedDemand = constrain(getActiveSpeed(), minSpeed, maxSpeed);
useAirSpeed = isAutoSpeedRequiredByNav() == FW_AUTO_SPD_AIR;
}

uint16_t actualSpeed = posControl.actualState.vel3D;
posControl.autoSpeedSpdSource = FW_AUTO_SPD_GROUND;
uint16_t groundSpeedBoost = 0;

#ifdef USE_PITOT
if (pitotValidateAirspeed()) {
static bool airspeedBoost = false;

// Pitot available and airspeed source selected or low airspeed boost applied when using ground speed source
if (!LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_AUTOSPEED_AIRSPEED) || airspeedBoost) {
if (useAirSpeed || airspeedBoost) {
actualSpeed = getAirspeedEstimate();
if (airspeedBoost && actualSpeed > minSpeed) {
airspeedBoost = false;
Expand Down
Loading