diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index ed5caf49548..ea62f7898be 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -105,6 +105,7 @@ typedef enum { TURTLE_MODE = (1 << 15), SOARING_MODE = (1 << 16), ANGLEHOLD_MODE = (1 << 17), + NAV_FW_AUTOLAND = (1 << 18) } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d913802d559..392c32cd10f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -594,7 +594,7 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) { // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle #ifdef USE_FW_AUTOLAND - if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !isFwLandInProgess()) { + if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && ! FLIGHT_MODE(NAV_FW_AUTOLAND)) { #else if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7e67faef01f..f0cc50bbaa1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -985,7 +985,7 @@ static const char * osdFailsafePhaseMessage(void) static const char * osdFailsafeInfoMessage(void) { - if (failsafeIsReceivingRxData()) { + if (failsafeIsReceivingRxData() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { // User must move sticks to exit FS mode return OSD_MESSAGE_STR(OSD_MSG_MOVE_EXIT_FS); } @@ -2277,7 +2277,7 @@ static bool osdDrawSingleElement(uint8_t item) { char *p = "ACRO"; #ifdef USE_FW_AUTOLAND - if (isFwLandInProgess()) + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) p = "LAND"; else #endif @@ -5142,7 +5142,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (ARMING_FLAG(ARMED)) { #ifdef USE_FW_AUTOLAND - if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) { + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || FLIGHT_MODE(NAV_FW_AUTOLAND)) { if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) { #else if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { @@ -5183,7 +5183,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter #ifdef USE_FW_AUTOLAND if (canFwLandCanceld()) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS); - } else if (!isFwLandInProgess()) { + } else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) { #endif const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index c040b7b762f..68170a90a9b 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -1059,6 +1059,11 @@ static bool djiFormatMessages(char *buff) if (FLIGHT_MODE(MANUAL_MODE)) { messages[messageCount++] = "(MANUAL)"; } + + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { + messages[messageCount++] = "(LAND)"; + } + } } // Pick one of the available messages. Each message lasts diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 46090ff24d0..fb0477005c3 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -64,6 +64,7 @@ #include "sensors/boardalignment.h" #include "sensors/battery.h" #include "sensors/gyro.h" +#include "sensors/diagnostics.h" #include "programming/global_variables.h" #include "sensors/rangefinder.h" @@ -1045,13 +1046,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, +/** Advanced Fixed Wing Autoland **/ #ifdef USE_FW_AUTOLAND [NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, - .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1071,8 +1073,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, - .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1092,8 +1094,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_APPROACH, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, - .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_WP, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1114,7 +1116,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_COURSE_HOLD_MODE, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1135,7 +1137,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FLARE, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_COURSE_HOLD_MODE, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1148,8 +1150,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1711,7 +1713,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF /* If position sensors unavailable - land immediately (wait for timeout on GPS) * Continue to check for RTH sanity during landing */ - if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) { + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (previousState != NAV_STATE_WAYPOINT_REACHED && !validateRTHSanityChecker())) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1721,7 +1723,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF #ifdef USE_FW_AUTOLAND if (STATE(AIRPLANE)) { - int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8; + int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8, approachSettingIdx = -1; #ifdef USE_MULTI_MISSION missionIdx = posControl.loadedMultiMissionIndex - 1; #endif @@ -1730,18 +1732,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF shIdx = posControl.safehomeState.index; missionFwLandConfigStartIdx = MAX_SAFE_HOMES; #endif - if (!posControl.fwLandState.landAborted && (shIdx >= 0 || missionIdx >= 0) && (fwAutolandApproachConfig(shIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(shIdx)->landApproachHeading2 != 0)) { + if (previousState == NAV_STATE_WAYPOINT_REACHED && missionIdx >= 0) { + approachSettingIdx = missionFwLandConfigStartIdx + missionIdx; + } else if (shIdx >= 0) { + approachSettingIdx = shIdx; + } + + if (!posControl.fwLandState.landAborted && approachSettingIdx >= 0 && (fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading2 != 0)) { if (previousState == NAV_STATE_WAYPOINT_REACHED) { posControl.fwLandState.landPos = posControl.activeWaypoint.pos; - posControl.fwLandState.approachSettingIdx = missionFwLandConfigStartIdx + missionIdx; posControl.fwLandState.landWp = true; } else { posControl.fwLandState.landPos = posControl.safehomeState.nearestSafeHome; - posControl.fwLandState.approachSettingIdx = shIdx; posControl.fwLandState.landWp = false; } + posControl.fwLandState.approachSettingIdx = approachSettingIdx; posControl.fwLandState.landAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt; posControl.fwLandState.landAproachAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; @@ -1813,6 +1820,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled - WP uses global altitude, not AGL +#ifdef USE_FW_AUTOLAND + if (previousState != NAV_STATE_FW_LANDING_ABORT) { + posControl.fwLandState.landAborted = false; + } +#endif + if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) { /* Use p3 as the volatile jump counter, allowing embedded, rearmed jumps Using p3 minimises the risk of saving an invalid counter if a mission is aborted */ @@ -2022,11 +2035,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig { UNUSED(previousState); +#ifdef USE_FW_AUTOLAND + if (posControl.fwLandState.landAborted) { + return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; + } +#endif + const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState); +#ifdef USE_FW_AUTOLAND if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) { return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; - } else if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) { + } else +#endif + if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) { return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; } else if (landEvent == NAV_FSM_EVENT_SUCCESS) { // Landing controller returned success - invoke RTH finishing state and finish the waypoint @@ -2400,7 +2422,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; } - if (getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) { + if (getHwRangefinderStatus() == HW_SENSOR_OK && getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) { posControl.fwLandState.landState = FW_AUTOLAND_STATE_FLARE; return NAV_FSM_EVENT_SUCCESS; } @@ -3016,7 +3038,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla updateDesiredRTHAltitude(); // Reset RTH sanity checker for new home position if RTH active - if (FLIGHT_MODE(NAV_RTH_MODE)) { + if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_FW_AUTOLAND) ) { initializeRTHSanityChecker(); } @@ -3138,7 +3160,7 @@ void updateHomePosition(void) static bool isHomeResetAllowed = false; // If pilot so desires he may reset home position to current position if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { - if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { + if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { homeUpdateFlags = 0; homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setHome = true; @@ -3243,7 +3265,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis) suspendTracking = false; } - if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) { + if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_FW_AUTOLAND) || !ARMING_FLAG(ARMED) || suspendTracking) { return; } @@ -4061,6 +4083,7 @@ bool isNavHoldPositionActive(void) // Also hold position during emergency landing if position valid return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || FLIGHT_MODE(NAV_POSHOLD_MODE) || + (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) || navigationIsExecutingAnEmergencyLanding(); } @@ -4169,7 +4192,7 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.flags.verticalPositionDataConsumed = false; #ifdef USE_FW_AUTOLAND - if (!isFwLandInProgess()) { + if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) { posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; } #endif @@ -4210,7 +4233,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -4361,35 +4384,15 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } - /* WP mission activation control: - * canActivateWaypoint & waypointWasActivated are used to prevent WP mission - * auto restarting after interruption by Manual or RTH modes. - * WP mode must be deselected before it can be reactivated again - * WP Mode also inhibited when Mission Planner is active */ - static bool waypointWasActivated = false; - bool canActivateWaypoint = isWaypointMissionValid(); - bool wpRthFallbackIsActive = false; - - if (IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive) { - canActivateWaypoint = false; - } else { - if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) { - canActivateWaypoint = false; - - if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { - canActivateWaypoint = true; - waypointWasActivated = false; - } - } - - wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint; +#ifdef USE_FW_AUTOLAND + if (isFwLandInProgess()) { + return NAV_FSM_EVENT_SWITCH_TO_RTH; } +#endif - /* Pilot-triggered RTH, also fall-back for WP if no mission is loaded. - * Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss - * Without this loss of any of the canActivateNavigation && canActivateAltHold - * will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back - * logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) */ + /* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded. + * WP prevented from falling back to RTH if WP mission planner is active */ + const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) { if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { return NAV_FSM_EVENT_SWITCH_TO_RTH; @@ -4412,6 +4415,12 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) waypointWasActivated = true; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; } + +#ifdef USE_FW_AUTOLAND + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { + return NAV_FSM_EVENT_SWITCH_TO_RTH; + } +#endif } if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) { @@ -4899,7 +4908,7 @@ void abortForcedRTH(void) rthState_e getStateOfForcedRTH(void) { /* If forced RTH activated and in AUTO_RTH or EMERG state */ - if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT))) { + if (posControl.flags.forcedRTHActivated && ((navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT)) || FLIGHT_MODE(NAV_FW_AUTOLAND))) { if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) { return RTH_HAS_LANDED; } @@ -4981,6 +4990,12 @@ bool navigationIsFlyingAutonomousMode(void) bool navigationRTHAllowsLanding(void) { +#ifdef USE_FW_AUTOLAND + if (posControl.fwLandState.landAborted) { + return false; + } +#endif + // WP mission RTH landing setting if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0; @@ -5138,15 +5153,6 @@ void resetFwAutolandApproach(int8_t idx) } } -bool isFwLandInProgess(void) -{ - return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER - || posControl.navState == NAV_STATE_FW_LANDING_LOITER - || posControl.navState == NAV_STATE_FW_LANDING_APPROACH - || posControl.navState == NAV_STATE_FW_LANDING_GLIDE - || posControl.navState == NAV_STATE_FW_LANDING_FLARE; -} - bool canFwLandCanceld(void) { return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 884f1726388..6ebdaeccead 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -693,7 +693,6 @@ uint8_t getActiveWpNumber(void); int32_t navigationGetHomeHeading(void); #ifdef USE_FW_AUTOLAND -bool isFwLandInProgess(void); bool canFwLandCanceld(void); #endif diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index da5d5f70c09..9e8daeafded 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -644,7 +644,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat // Manual throttle increase #ifdef USE_FW_AUTOLAND - if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !isFwLandInProgess()) { + if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { #else if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { #endif diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 8fcca4dabcd..2b10896b718 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -746,7 +746,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1 #ifdef USE_FW_AUTOLAND - return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || isFwLandInProgess()) ? 1 : 0; + return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || FLIGHT_MODE(NAV_FW_AUTOLAND)) ? 1 : 0; #else return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0; #endif diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index af9f278659f..aee7e5d3037 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -371,6 +371,11 @@ static void crsfFrameFlightMode(sbuf_t *dst) } else if (FLIGHT_MODE(ANGLEHOLD_MODE)) { flightMode = "ANGH"; } +#ifdef USE_FW_AUTOLAND + } else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { + flightMode = "LAND" + } +#endif #ifdef USE_GPS } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) { flightMode = "WAIT"; // Waiting for GPS lock diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 00e99a80472..393ebf76068 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -178,6 +178,10 @@ void ltm_sframe(sbuf_t *dst) lt_flightmode = LTM_MODE_ANGLE; else if (FLIGHT_MODE(HORIZON_MODE)) lt_flightmode = LTM_MODE_HORIZON; +#ifdef USE_FW_AUTOLAND + else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) + lt_flightmode = LTM_MODE_LAND; +#endif else lt_flightmode = LTM_MODE_RATE; // Rate mode