From acfd1cdbb84a8ddfaa2fe6d42c7a130e86a4652c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 30 Jul 2026 22:24:23 +0100 Subject: [PATCH 1/3] enable fw autospeed from wp mode --- src/main/io/osd.c | 2 +- src/main/navigation/navigation.c | 40 +++++++++++--------- src/main/navigation/navigation_fixedwing.c | 43 ++++++++++++++++++---- 3 files changed, 59 insertions(+), 26 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 500140a3bbd..35927944d31 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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()) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f285d50e15d..354b7a48f1f 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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 = 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 + return MIN(wpSpecificSpeed, navConfig()->general.max_auto_speed); } } @@ -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) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 1db29a37330..5058ab1934b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -74,6 +74,7 @@ static bool isAutoThrottleManuallyIncreased = false; static float navCrossTrackError; static int8_t loiterDirYaw = 1; bool needToCalculateCircularLoiter; +bool autoSpeedIsActive = false; // Calculates the cutoff frequency for smoothing out roll/pitch commands // control_smoothness valid range from 0 to 9 @@ -887,18 +888,38 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) * Auto Speed mode control *-----------------------------------------------------------*/ bool isFixedwingAutoSpeedActive(void) +{ + return autoSpeedIsActive; +} + +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; @@ -918,7 +939,14 @@ 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; @@ -926,8 +954,9 @@ void applyAutoSpeedThrottleDemand(int16_t *throttleCommand, timeUs_t currentTime #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; From 0cd74ce2c1dd251e9ef8d5611c0901ecd13eb150 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 2 Aug 2026 10:41:44 +0100 Subject: [PATCH 2/3] fixes --- src/main/navigation/navigation.c | 4 ++-- src/main/navigation/navigation_fixedwing.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 354b7a48f1f..e112ce8a1b9 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4337,9 +4337,9 @@ float getActiveSpeed(void) 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 + wpSpecificSpeed = ABS(posControl.waypointList[posControl.activeWaypointIndex].p2); // P1 is hold time } else { - wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case + wpSpecificSpeed = ABS(posControl.waypointList[posControl.activeWaypointIndex].p1); // default case } if (STATE(AIRPLANE)) { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 5058ab1934b..ceda1d174b3 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -73,8 +73,8 @@ static float throttleSpeedAdjustment = 0; static bool isAutoThrottleManuallyIncreased = false; static float navCrossTrackError; static int8_t loiterDirYaw = 1; -bool needToCalculateCircularLoiter; -bool autoSpeedIsActive = false; +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 From 5470b9e8f64c6ad94767eba237d8452aaff25c37 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 3 Aug 2026 17:19:53 +0100 Subject: [PATCH 3/3] Update Navigation.md --- docs/Navigation.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/Navigation.md b/docs/Navigation.md index 4d7b9740d91..a0b69d7d87a 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -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. @@ -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: @@ -83,9 +83,9 @@ Parameters: * `` - Altitude in cm. See `p3` bit 0 for datum definition. - * `` - 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. + * `` - 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. - * `` - 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. + * `` - 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. * `` - 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).