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 2 commits into
base: maintenance-10.x
Choose a base branch
from
+60 −27
Open

Enable Fixed Wing Auto Speed During Waypoint Mission #11757

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 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
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
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