Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

Merge branch 'release_7.1.0' into master_7.1.0

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2024-03-19 09:11:11 +01:00
commit ae5869c0bc
19 changed files with 413 additions and 97 deletions
Download patch file Download diff file Expand all files Collapse all files

150
src/main/navigation/navigation.c
View file

@ -65,6 +65,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"
@ -1044,13 +1045,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 = {
@ -1070,8 +1072,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 = {
@ -1091,8 +1093,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 = {
@ -1113,7 +1115,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 = {
@ -1134,7 +1136,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 = {
@ -1147,8 +1149,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 = {
@ -1677,7 +1679,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;
}
@ -1687,7 +1689,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
@ -1696,18 +1698,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;
@ -1779,6 +1786,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 */
@ -1988,11 +2001,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
@ -2322,7 +2344,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING);
//disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
@ -2366,14 +2388,14 @@ 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;
}
if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING);
//disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
@ -2387,7 +2409,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING);
//disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SUCCESS;
}
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
@ -2993,7 +3015,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();
}
@ -3115,7 +3137,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;
@ -3935,6 +3957,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();
}
@ -3974,7 +3997,9 @@ bool isWaypointNavTrackingActive(void)
// is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
// (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex);
return FLIGHT_MODE(NAV_WP_MODE)
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|| (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
}
/*-----------------------------------------------------------
@ -4039,7 +4064,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
@ -4080,7 +4105,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);
}
@ -4192,22 +4217,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
posControl.rthSanityChecker.rthSanityOK = true;
/* 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. */
static bool waypointWasActivated = false;
const bool isWpMissionLoaded = isWaypointMissionValid();
bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner
if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) {
canActivateWaypoint = false;
if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
canActivateWaypoint = true;
waypointWasActivated = false;
}
}
/* Airplane specific modes */
if (STATE(AIRPLANE)) {
// LAUNCH mode has priority over any other NAV mode
@ -4247,14 +4256,36 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
/* 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;
/* 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;
}
/* 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) */
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) {
// 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)
if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
@ -4268,11 +4299,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
// Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded.
// Also check multimission mission change status before activating WP mode.
#ifdef USE_MULTI_MISSION
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP)) {
#else
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
#endif
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
waypointWasActivated = true;
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
}
@ -4767,7 +4798,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;
}
@ -4849,6 +4880,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;
@ -5006,15 +5043,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