diff --git a/docs/Settings.md b/docs/Settings.md index 7ed9437ced1..adb371d17a0 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1814,11 +1814,11 @@ Allows to chose when the home position is reset. Can help prevent resetting home ### inav_use_gps_no_baro -_// TODO_ +Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed. | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| ON | OFF | ON | --- diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b16f03de775..727488cc1b1 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1308,10 +1308,16 @@ static void writeSlowFrame(void) static void loadSlowState(blackboxSlowState_t *slow) { memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags; - // Also log Nav auto selected flight modes rather than just those selected by boxmode - if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) { + // Also log Nav auto enabled flight modes rather than just those selected by boxmode + if (FLIGHT_MODE(ANGLE_MODE)) { slow->flightModeFlags |= (1 << BOXANGLE); } + if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { + slow->flightModeFlags |= (1 << BOXNAVALTHOLD); + } + if (FLIGHT_MODE(NAV_RTH_MODE)) { + slow->flightModeFlags |= (1 << BOXNAVRTH); + } if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) { slow->flightModeFlags |= (1 << BOXHEADINGHOLD); } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index bede8929fea..4b231548b99 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -342,6 +342,10 @@ static void updateArmingStatus(void) DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); } + if (ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED) && !IS_RC_MODE_ACTIVE(BOXARM)) { + DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); + } + /* CHECK: Arming switch */ // If arming is disabled and the ARM switch is on // Note that this should be last check so all other blockers could be cleared correctly diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index b1352c1ce27..f2b368b8fcb 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -60,7 +60,6 @@ #include "drivers/pwm_esc_detect.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" -#include "drivers/pwm_output.h" #include "drivers/sensor.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c00ba9cde5d..334b128741f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2266,9 +2266,10 @@ groups: min: 0 max: 255 - name: inav_use_gps_no_baro + description: "Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed." field: use_gps_no_baro type: bool - default_value: OFF + default_value: ON - name: inav_allow_dead_reckoning description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation" default_value: OFF diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 15aac6a1d60..56245ea04af 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1022,7 +1022,7 @@ static const char * divertingToSafehomeMessage(void) return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); } #endif - return NULL; + return NULL; } @@ -1047,7 +1047,7 @@ static const char * navigationStateMessage(void) linearDescentMessageMs = millis() + 5000; // Show message for 5 seconds. return OSD_MESSAGE_STR(OSD_MSG_RTH_LINEAR_DESCENT); - } else + } else return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); } case MW_NAV_STATE_HOLD_INFINIT: @@ -1088,7 +1088,7 @@ static const char * navigationStateMessage(void) // If there is a FS landing delay occurring. That is handled by the calling function. if (posControl.landingDelay > 0) break; - + return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND); } case MW_NAV_STATE_LAND_START_DESCENT: @@ -2401,7 +2401,7 @@ static bool osdDrawSingleElement(uint8_t item) { char *p = "ACRO"; #ifdef USE_FW_AUTOLAND - if (FLIGHT_MODE(NAV_FW_AUTOLAND)) + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) p = "LAND"; else #endif @@ -5404,6 +5404,7 @@ static void osdRefresh(timeUs_t currentTimeUs) static uint8_t statsCurrentPage = 0; static bool statsDisplayed = false; static bool statsAutoPagingEnabled = true; + static bool isThrottleHigh = false; // Detect arm/disarm if (armState != ARMING_FLAG(ARMED)) { @@ -5429,6 +5430,7 @@ static void osdRefresh(timeUs_t currentTimeUs) statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false; osdShowStats(statsSinglePageCompatible, statsCurrentPage); osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); + isThrottleHigh = checkStickPosition(THR_HI); } armState = ARMING_FLAG(ARMED); @@ -5494,7 +5496,7 @@ static void osdRefresh(timeUs_t currentTimeUs) } // Handle events when either "Splash", "Armed" or "Stats" screens are displayed. - if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { + if (currentTimeUs > resumeRefreshAt || (OSD_RESUME_UPDATES_STICK_COMMAND && !isThrottleHigh)) { // Time elapsed or canceled by stick commands. // Exit to normal OSD operation. displayClearScreen(osdDisplayPort); @@ -5503,6 +5505,7 @@ static void osdRefresh(timeUs_t currentTimeUs) } else { // Continue "Splash", "Armed" or "Stats" screens. displayHeartbeat(osdDisplayPort); + isThrottleHigh = checkStickPosition(THR_HI); } return; @@ -5717,11 +5720,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec); messages[messageCount++] = messageBuf; - } + } else { #ifdef USE_FW_AUTOLAND - if (canFwLandCanceld()) { + if (canFwLandingBeCancelled()) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS); } else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) { #endif diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e7261b068cf..711ee05f37a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -327,8 +327,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState); @@ -355,6 +355,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState); #endif @@ -422,7 +423,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE, .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE, .timeoutMs = 0, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, @@ -437,7 +438,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, @@ -584,7 +585,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK] = NAV_STATE_RTH_TRACKBACK, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -593,7 +594,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT, .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, .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, // allow pos adjustment while climbind to safe alt + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbing to safe alt .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_CLIMB, .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT, @@ -639,7 +640,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwError = MW_NAV_ERROR_NONE, .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, @@ -650,37 +651,37 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - [NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = { - .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING, - .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING] = { + .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING, + .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, .timeoutMs = 500, - .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, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_SETTLE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, - [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, - [NAV_STATE_RTH_HOVER_ABOVE_HOME] = { - .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME, - .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME, + [NAV_STATE_RTH_LOITER_ABOVE_HOME] = { + .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME, + .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME, .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 | NAV_RC_ALT, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LOITER_ABOVE_HOME, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, @@ -694,20 +695,20 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_LANDING, .onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, - [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, + [NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME, } }, @@ -715,7 +716,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_FINISHING, .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING, .timeoutMs = 0, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, @@ -826,7 +827,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold? .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_TIMED, .mwError = MW_NAV_ERROR_NONE, @@ -847,7 +848,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, @@ -885,7 +886,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_FINISH, @@ -924,7 +925,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE, + .stateFlags = NAV_CTL_HOLD | NAV_CTL_EMERG | NAV_REQUIRE_ANGLE, .mapToFlightModes = 0, .mwState = MW_NAV_STATE_EMERGENCY_LANDING, .mwError = MW_NAV_ERROR_LANDING, @@ -942,7 +943,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED, .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED, .timeoutMs = 10, - .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE, + .stateFlags = NAV_CTL_HOLD | NAV_CTL_EMERG | NAV_REQUIRE_ANGLE, .mapToFlightModes = 0, .mwState = MW_NAV_STATE_LANDED, .mwError = MW_NAV_ERROR_LANDING, @@ -1051,7 +1052,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .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_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, @@ -1072,7 +1073,7 @@ 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_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, @@ -1106,6 +1107,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT, } }, @@ -1127,6 +1129,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT, } }, @@ -1140,11 +1143,26 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FLARE, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FLARE, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + + [NAV_STATE_FW_LANDING_FINISHED] = { + .persistentId = NAV_PERSISTENT_ID_FW_LANDING_FINISHED, + .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED, + .timeoutMs = 10, + .stateFlags = NAV_REQUIRE_ANGLE, + .mapToFlightModes = NAV_FW_AUTOLAND, + .mwState = MW_NAV_STATE_LANDED, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FINISHED, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, + [NAV_STATE_FW_LANDING_ABORT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT, @@ -1245,7 +1263,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n } // Prepare position controller if idle or current Mode NOT active in position hold state - if (previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME && + if (previousState != NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_LOITER_ABOVE_HOME && previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND && previousState != NAV_STATE_WAYPOINT_FINISHED && previousState != NAV_STATE_WAYPOINT_HOLD_TIME) { @@ -1426,10 +1444,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING - } else { - if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) { - rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference + return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING + } + else { + // Switch to RTH trackback + bool trackbackActive = navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || + (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated); + + if (trackbackActive && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) { + rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference posControl.flags.rthTrackbackActive = true; calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition()); return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK; @@ -1495,8 +1518,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n initializeRTHSanityChecker(); } - // Save initial home distance for future use + // Save initial home distance and direction for future use posControl.rthState.rthInitialDistance = posControl.homeDistance; + posControl.activeWaypoint.bearing = posControl.homeDirection; fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) { @@ -1582,7 +1606,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio if ((posControl.flags.estPosStatus >= EST_USABLE)) { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); - if (isWaypointReached(tmpHomePos, 0)) { + if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { // Successfully reached position target - update XYZ-position setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); @@ -1591,7 +1615,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) posControl.rthState.rthLinearDescentActive = false; - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING } else { setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); return NAV_FSM_EVENT_NONE; @@ -1605,7 +1629,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_NONE; } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState) { UNUSED(previousState); @@ -1639,7 +1663,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) { resetLandingDetector(); // force reset landing detector just in case updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land + return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land } else { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); @@ -1647,7 +1671,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState) { UNUSED(previousState); @@ -1656,7 +1680,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER); + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LOITER); setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); return NAV_FSM_EVENT_NONE; @@ -1664,9 +1688,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState) { -#ifndef USE_FW_AUTOLAND UNUSED(previousState); -#endif //On ROVER and BOAT we immediately switch to the next event if (!STATE(ALTITUDE_CONTROL)) { @@ -1679,7 +1701,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() || (previousState != NAV_STATE_WAYPOINT_REACHED && !validateRTHSanityChecker())) { + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (FLIGHT_MODE(NAV_RTH_MODE) && !validateRTHSanityChecker())) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1698,7 +1720,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF shIdx = posControl.safehomeState.index; missionFwLandConfigStartIdx = MAX_SAFE_HOMES; #endif - if (previousState == NAV_STATE_WAYPOINT_REACHED && missionIdx >= 0) { + if (FLIGHT_MODE(NAV_WP_MODE) && missionIdx >= 0) { approachSettingIdx = missionFwLandConfigStartIdx + missionIdx; } else if (shIdx >= 0) { approachSettingIdx = shIdx; @@ -1706,7 +1728,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF if (!posControl.fwLandState.landAborted && approachSettingIdx >= 0 && (fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading2 != 0)) { - if (previousState == NAV_STATE_WAYPOINT_REACHED) { + if (FLIGHT_MODE(NAV_WP_MODE)) { posControl.fwLandState.landPos = posControl.activeWaypoint.pos; posControl.fwLandState.landWp = true; } else { @@ -1999,8 +2021,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState) { - UNUSED(previousState); - #ifdef USE_FW_AUTOLAND if (posControl.fwLandState.landAborted) { return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; @@ -2009,21 +2029,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig 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 -#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 + if (landEvent == NAV_FSM_EVENT_SUCCESS) { + // Landing controller returned success - invoke RTH finish states and finish the waypoint navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState); - return NAV_FSM_EVENT_SUCCESS; - } - else { - return NAV_FSM_EVENT_NONE; + navOnEnteringState_NAV_STATE_RTH_FINISHED(previousState); } + + return landEvent; } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState) @@ -2334,6 +2346,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav { UNUSED(previousState); + if (STATE(LANDING_DETECTED)) { + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED; + } + if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -2342,12 +2358,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; } - if (isLandingDetected()) { - posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; - //disarm(DISARM_LANDING); - return NAV_FSM_EVENT_SWITCH_TO_IDLE; - } - if (getLandAltitude() <= fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) { resetPositionController(); posControl.cruise.course = posControl.fwLandState.landingDirection; @@ -2384,6 +2394,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga { UNUSED(previousState); + if (STATE(LANDING_DETECTED)) { + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED; + } + if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; } @@ -2393,12 +2407,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga return NAV_FSM_EVENT_SUCCESS; } - if (isLandingDetected()) { - posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; - //disarm(DISARM_LANDING); - return NAV_FSM_EVENT_SWITCH_TO_IDLE; - } - setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_NONE; } @@ -2407,16 +2415,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga { UNUSED(previousState); - if (isLandingDetected()) { - posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; - //disarm(DISARM_LANDING); - return NAV_FSM_EVENT_SUCCESS; + if (STATE(LANDING_DETECTED)) { + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED; } + setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_NONE; } +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; + + return NAV_FSM_EVENT_NONE; +} + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState) { UNUSED(previousState); @@ -2533,7 +2549,7 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; break; - case RTH_HOME_FINAL_HOVER: + case RTH_HOME_FINAL_LOITER: if (navConfig()->general.rth_home_altitude) { posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude; } @@ -2818,30 +2834,28 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos) /*----------------------------------------------------------- * Check if waypoint is/was reached. - * waypointBearing stores initial bearing to waypoint + * 'waypointBearing' stores initial bearing to waypoint. *-----------------------------------------------------------*/ bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing) { posControl.wpDistance = calculateDistanceToDestination(waypointPos); - // Airplane will do a circular loiter at hold waypoints and might never approach them closer than waypoint_radius - // Check within 10% margin of circular loiter radius - if (STATE(AIRPLANE) && isNavHoldPositionActive() && posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)) { - return true; - } + // Check if waypoint was missed based on bearing to waypoint exceeding given angular limit relative to initial waypoint bearing. + // Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active + uint16_t relativeBearingTargetAngle = 10000; - if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) { - // If WP turn smoothing CUT option used WP is reached when start of turn is initiated - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) { + if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) { + // If WP mode turn smoothing CUT option used waypoint is reached when start of turn is initiated + if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) { posControl.flags.wpTurnSmoothingActive = false; return true; } - // Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw - // Same method for turn smoothing option but relative bearing set at 60 degrees - uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000; - if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) { - return true; - } + relativeBearingTargetAngle = 6000; + } + + + if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingTargetAngle) { + return true; } return posControl.wpDistance <= (navConfig()->general.waypoint_radius); @@ -3327,9 +3341,6 @@ void updateLandingStatus(timeMs_t currentTimeMs) } resetLandingDetector(); - if (!IS_RC_MODE_ACTIVE(BOXARM)) { - DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); - } return; } @@ -3949,16 +3960,20 @@ bool isLastMissionWaypoint(void) /* Checks if Nav hold position is active */ bool isNavHoldPositionActive(void) { - // WP mode last WP hold and Timed hold positions + /* If the current Nav state isn't flagged as a hold point (NAV_CTL_HOLD) then + * waypoints are assumed to be hold points by default unless excluded as defined here */ + + if (navGetCurrentStateFlags() & NAV_CTL_HOLD) { + return true; + } + + // No hold required for basic WP type unless it's the last mission waypoint if (FLIGHT_MODE(NAV_WP_MODE)) { - return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED; + return posControl.waypointList[posControl.activeWaypointIndex].action != NAV_WP_ACTION_WAYPOINT || isLastMissionWaypoint(); } - // RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode - // 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(); + + // No hold required for Trackback WPs or for fixed wing autoland WPs not flagged as hold points (returned above if they are) + return !FLIGHT_MODE(NAV_FW_AUTOLAND) && !posControl.flags.rthTrackbackActive; } float getActiveSpeed(void) @@ -3997,7 +4012,7 @@ 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) + return FLIGHT_MODE(NAV_WP_MODE) || posControl.navState == NAV_STATE_FW_LANDING_APPROACH || (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex); } @@ -4797,9 +4812,9 @@ void abortForcedRTH(void) rthState_e getStateOfForcedRTH(void) { - /* If forced RTH activated and in AUTO_RTH or EMERG state */ + /* If forced RTH activated and in AUTO_RTH, EMERG state or FW Auto Landing */ 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) { + if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED || posControl.navState == NAV_STATE_FW_LANDING_FINISHED) { return RTH_HAS_LANDED; } else { @@ -4885,7 +4900,7 @@ bool navigationRTHAllowsLanding(void) return false; } #endif - + // WP mission RTH landing setting if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0; @@ -5043,12 +5058,9 @@ void resetFwAutolandApproach(int8_t idx) } } -bool canFwLandCanceld(void) +bool canFwLandingBeCancelled(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; + return FLIGHT_MODE(NAV_FW_AUTOLAND) && posControl.navState != NAV_STATE_FW_LANDING_FLARE; } #endif diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 98bf6c4d62c..042c20de8ea 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -702,7 +702,7 @@ uint8_t getActiveWpNumber(void); int32_t navigationGetHomeHeading(void); #ifdef USE_FW_AUTOLAND -bool canFwLandCanceld(void); +bool canFwLandingBeCancelled(void); #endif /* Compatibility data */ diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 49fbf11ab5b..46cc6c95822 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -290,9 +290,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // Detemine if a circular loiter is required. // For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target #define TAN_15DEG 0.26795f - needToCalculateCircularLoiter = isNavHoldPositionActive() && - (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) && - (distanceToActualTarget > 50.0f); + + bool loiterApproachActive = isNavHoldPositionActive() && + distanceToActualTarget <= (navLoiterRadius / TAN_15DEG) && + distanceToActualTarget > 50.0f; + needToCalculateCircularLoiter = loiterApproachActive || (navGetCurrentStateFlags() & NAV_CTL_HOLD); + //if vtol landing is required, fly straight to homepoint if ((posControl.navState == NAV_STATE_RTH_HEAD_HOME) && navigationRTHAllowsLanding() && checkMixerATRequired(MIXERAT_REQUEST_LAND)){ needToCalculateCircularLoiter = false; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index ca0cea8a3d1..da44e6e76b2 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -163,16 +163,18 @@ typedef enum { NAV_FSM_EVENT_STATE_SPECIFIC_3, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_4, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_5, // State-specific event - NAV_FSM_EVENT_STATE_SPECIFIC_6, // State-specific event + + NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_1, + NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, + NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_1, + NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_2, + NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_3, + NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_1, + NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3, - NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1, - NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, - NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, - NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3, - NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4, - NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_5, - NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_6, - NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_6, + NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4, + NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5, + NAV_FSM_EVENT_COUNT, } navigationFSMEvent_t; @@ -195,7 +197,7 @@ typedef enum { NAV_PERSISTENT_ID_RTH_INITIALIZE = 8, NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT = 9, NAV_PERSISTENT_ID_RTH_HEAD_HOME = 10, - NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING = 11, + NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING = 11, NAV_PERSISTENT_ID_RTH_LANDING = 12, NAV_PERSISTENT_ID_RTH_FINISHING = 13, NAV_PERSISTENT_ID_RTH_FINISHED = 14, @@ -226,7 +228,7 @@ typedef enum { NAV_PERSISTENT_ID_CRUISE_ADJUSTING = 34, NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, - NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, + NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME = 36, NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME NAV_PERSISTENT_ID_RTH_TRACKBACK = 38, @@ -239,6 +241,7 @@ typedef enum { NAV_PERSISTENT_ID_FW_LANDING_GLIDE = 45, NAV_PERSISTENT_ID_FW_LANDING_FLARE = 46, NAV_PERSISTENT_ID_FW_LANDING_ABORT = 47, + NAV_PERSISTENT_ID_FW_LANDING_FINISHED = 48, } navigationPersistentId_e; typedef enum { @@ -256,8 +259,8 @@ typedef enum { NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, NAV_STATE_RTH_TRACKBACK, NAV_STATE_RTH_HEAD_HOME, - NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - NAV_STATE_RTH_HOVER_ABOVE_HOME, + NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, + NAV_STATE_RTH_LOITER_ABOVE_HOME, NAV_STATE_RTH_LANDING, NAV_STATE_RTH_FINISHING, NAV_STATE_RTH_FINISHED, @@ -285,12 +288,13 @@ typedef enum { NAV_STATE_CRUISE_INITIALIZE, NAV_STATE_CRUISE_IN_PROGRESS, NAV_STATE_CRUISE_ADJUSTING, - + NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, NAV_STATE_FW_LANDING_LOITER, NAV_STATE_FW_LANDING_APPROACH, NAV_STATE_FW_LANDING_GLIDE, NAV_STATE_FW_LANDING_FLARE, + NAV_STATE_FW_LANDING_FINISHED, NAV_STATE_FW_LANDING_ABORT, NAV_STATE_MIXERAT_INITIALIZE, @@ -325,9 +329,10 @@ typedef enum { /* Additional flags */ NAV_CTL_LAND = (1 << 14), - NAV_AUTO_WP_DONE = (1 << 15), //Waypoint mission reached the last waypoint and is idling + NAV_AUTO_WP_DONE = (1 << 15), // Waypoint mission reached the last waypoint and is idling - NAV_MIXERAT = (1 << 16), //MIXERAT in progress + NAV_MIXERAT = (1 << 16), // MIXERAT in progress + NAV_CTL_HOLD = (1 << 17), // Nav loiter active at position } navigationFSMStateFlags_t; typedef struct { @@ -394,7 +399,7 @@ typedef enum { RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach RTH_HOME_ENROUTE_PROPORTIONAL, // Prorpotional position for RTH approach RTH_HOME_ENROUTE_FINAL, // Final position for RTH approach - RTH_HOME_FINAL_HOVER, // Final hover altitude (if rth_home_altitude is set) + RTH_HOME_FINAL_LOITER, // Final loiter altitude (if rth_home_altitude is set) RTH_HOME_FINAL_LAND, // Home position and altitude } rthTargetMode_e;