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..e112ce8a1b9 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 = 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 + 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..ceda1d174b3 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -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 @@ -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;