From cd67c181f017312ee3c37948844151e9ea9fee3d Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 12 May 2026 17:00:23 +0300 Subject: [PATCH] implement vtol mission auto transition --- docs/MixerProfile.md | 22 +++ docs/Navigation.md | 31 ++++ src/main/fc/settings.yaml | 26 ++++ src/main/flight/mixer_profile.c | 62 ++++++-- src/main/flight/mixer_profile.h | 5 +- src/main/navigation/navigation.c | 182 ++++++++++++++++++++++- src/main/navigation/navigation.h | 11 ++ src/main/navigation/navigation_private.h | 1 + 8 files changed, 320 insertions(+), 20 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 685e17a4e70..1fdd9132dad 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -72,6 +72,28 @@ This feature is mainly for RTH in a failsafe event. When set properly, model wil Set `mixer_automated_switch` to `ON` in mixer_profile for MC mode. Set `mixer_switch_trans_timer` in mixer_profile for MC mode for the time required to gain airspeed for your model before entering to FW mode. When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Model will not perform automated transition at all. +### Mission-authorized VTOL transition (waypoint User Action) + +INAV supports mission-requested VTOL transitions through the existing automated transition path. This is configured with: + +- `nav_vtol_mission_transition_user_action` (`OFF`, `USER1`, `USER2`, `USER3`, `USER4`) +- `nav_vtol_mission_transition_min_altitude_cm` (optional, `0` disables minimum-altitude check) +- `mixer_switch_trans_airspeed_cm_s` (optional, airspeed-based MC->FW switch threshold) + +On each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`), the configured USER action bit is used as absolute target selector: + +- selected USER bit = `0` -> transition to MC / MULTIROTOR profile +- selected USER bit = `1` -> transition to FW / AIRPLANE profile +- This is **not** a toggle command. +- If already in the requested profile type, the action is treated as complete (idempotent). + +The mission pauses while transition is in progress and resumes after completion. + +For MC -> FW mission transitions, navigation uses a straight acceleration segment (no loiter) to build speed before hot-switch. +When `mixer_switch_trans_airspeed_cm_s > 0` and valid pitot airspeed is available, automated MC->FW switching uses this airspeed target. If airspeed is unavailable, transition falls back to `mixer_switch_trans_timer`. + +Manual RC switching (`MIXER PROFILE 2`, `MIXER TRANSITION`) remains blocked during normal active navigation. Mission VTOL transition does not bypass the hot-switch safety guard; it only authorizes switching inside the automated transition state. + ## TailSitter (planned for INAV 7.1) TailSitter is supported by add a 90deg offset to the board alignment. Set the board aliment normally in the mixer_profile for FW mode(`set platform_type = AIRPLANE`), The motor trust axis should be same direction as the airplane nose. Then, in the mixer_profile for takeoff and landing set `tailsitter_orientation_offset = ON ` to apply orientation offset. orientation offset will also add a 45deg orientation offset. diff --git a/docs/Navigation.md b/docs/Navigation.md index 4d7b9740d91..c716925ba77 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -102,6 +102,37 @@ Parameters: * `` - Last waypoint must have `flag` set to 165 (0xA5). +### Mission VTOL transition using existing User Actions + +Mission VTOL transition can be requested. + +Configuration: + +- `nav_vtol_mission_transition_user_action` selects which waypoint User Action (`USER1..USER4`) is used as the mission VTOL target selector. +- `nav_vtol_mission_transition_min_altitude_cm` optionally enforces a minimum altitude before transition start (`0` disables check). +- `nav_vtol_mission_transition_track_distance_cm` configures straight-line MC->FW transition guidance distance. +- `mixer_switch_trans_airspeed_cm_s` configures MC->FW airspeed threshold for automated profile switching. + +Behavior on each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`): + +- The configured USER bit is an **absolute target selector**: + - `0`: transition to MC / MULTIROTOR profile + - `1`: transition to FW / AIRPLANE profile +- This command is **not** a toggle. +- The command is idempotent: if already in the requested target profile type, the mission continues immediately. +- If a transition is needed, mission progression pauses while automated transition runs, then resumes only after completion. + +Transition behavior in this MVP: + +- MC -> FW: straight-line acceleration segment (no loiter), heading from the next waypoint bearing when available, otherwise current heading. +- MC -> FW switch point: if valid pitot airspeed is available and `mixer_switch_trans_airspeed_cm_s > 0`, switch to FW occurs at/above the configured airspeed. If airspeed is unavailable, timer-based fallback (`mixer_switch_trans_timer`) is used. +- FW -> MC: mission pauses during automated transition, then resumes after switching back to MC profile. +- Strict altitude hold is not enforced during MC -> FW transition; natural climb is allowed. + +Safety and scope: + +- This path uses authorized automated transition state handling; it does not permit manual mixer profile switching during normal waypoint navigation. + `wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`). `wp reset` - Resets the list, sets the number of waypoints to 0 and marks the list as invalid (but doesn't delete the waypoint definitions). diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c53ef66852f..44e34452fee 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -171,6 +171,9 @@ tables: - name: nav_wp_mission_restart enum: navMissionRestart_e values: ["START", "RESUME", "SWITCH"] + - name: nav_wp_user_action + enum: navMissionUserAction_e + values: ["OFF", "USER1", "USER2", "USER3", "USER4"] - name: djiRssiSource values: ["RSSI", "CRSF_LQ"] enum: djiRssiSource_e @@ -1277,6 +1280,12 @@ groups: field: mixer_config.switchTransitionTimer min: 0 max: 200 + - name: mixer_switch_trans_airspeed_cm_s + description: "Airspeed threshold [cm/s] for MC->FW automated profile switch. If > 0 and valid pitot airspeed is available, transition will switch to FW only after this speed is reached. If airspeed is unavailable, timer-based fallback (`mixer_switch_trans_timer`) is used." + default_value: 0 + field: mixer_config.switchTransitionAirspeed + min: 0 + max: 10000 - name: tailsitter_orientation_offset description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode" default_value: OFF @@ -2623,6 +2632,23 @@ groups: default_value: "RESUME" field: general.flags.waypoint_mission_restart table: nav_wp_mission_restart + - name: nav_vtol_mission_transition_user_action + description: "Selects which waypoint USER action bit (`USER1`..`USER4`) is used as mission VTOL target selector. OFF disables this feature. On navigable mission waypoints: selected USER bit = 1 requests FW profile, selected USER bit = 0 requests MC profile." + default_value: "OFF" + field: general.vtol_mission_transition_user_action + table: nav_wp_user_action + - name: nav_vtol_mission_transition_min_altitude_cm + description: "Minimum altitude [cm] required to start a mission-authorized VTOL transition. Set to 0 to disable the minimum-altitude check." + default_value: 0 + field: general.vtol_mission_transition_min_altitude + min: 0 + max: 50000 + - name: nav_vtol_mission_transition_track_distance_cm + description: "Straight-line target distance [cm] used during mission-authorized MC->FW transition guidance. This controls how far ahead the transition heading target is placed." + default_value: 100000 + field: general.vtol_mission_transition_track_distance + min: 1000 + max: 500000 - name: nav_wp_multi_mission_index description: "Index of active mission selected from multi mission WP entry loaded in flight controller. Limited to a maximum of 9 missions." default_value: 1 diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index a39fbfeedd9..4362f418476 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -17,6 +17,7 @@ #include "flight/failsafe.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "sensors/pitotmeter.h" #include "fc/fc_core.h" #include "fc/config.h" @@ -37,7 +38,7 @@ bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; int nextMixerProfileIndex; -PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 2); void pgResetFn_mixerProfiles(mixerProfile_t *instance) { @@ -53,6 +54,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .controlProfileLinking = SETTING_MIXER_CONTROL_PROFILE_LINKING_DEFAULT, .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, + .switchTransitionAirspeed = SETTING_MIXER_SWITCH_TRANS_AIRSPEED_CM_S_DEFAULT, .tailsitterOrientationOffset = SETTING_TAILSITTER_ORIENTATION_OFFSET_DEFAULT, .transition_PID_mmix_multiplier_roll = SETTING_TRANSITION_PID_MMIX_MULTIPLIER_ROLL_DEFAULT, .transition_PID_mmix_multiplier_pitch = SETTING_TRANSITION_PID_MMIX_MULTIPLIER_PITCH_DEFAULT, @@ -112,6 +114,25 @@ void setMixerProfileAT(void) mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; } +static bool requestTransitionsToFixedWing(const mixerProfileATRequest_e required_action) +{ + return required_action == MIXERAT_REQUEST_RTH || required_action == MIXERAT_REQUEST_MISSION_TO_FW; +} + +static bool mixerATReadyForHotSwitch(const mixerProfileATRequest_e required_action) +{ +#ifdef USE_PITOT + if (requestTransitionsToFixedWing(required_action) && + currentMixerConfig.switchTransitionAirspeed > 0 && + pitotValidForAirspeed()) { + return getAirspeedEstimate() >= currentMixerConfig.switchTransitionAirspeed; + } +#endif + + // Timer remains a fallback when airspeed is not configured/available. + return millis() > mixerProfileAT.transitionTransEndTime; +} + bool platformTypeConfigured(flyingPlatformType_e platformType) { if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){ @@ -120,6 +141,18 @@ bool platformTypeConfigured(flyingPlatformType_e platformType) return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType; } +static bool missionTransitionToMultirotorTypeConfigured(void) +{ + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) { + return false; + } + + const flyingPlatformType_e nextPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; + return nextPlatformType == PLATFORM_MULTIROTOR || + nextPlatformType == PLATFORM_TRICOPTER || + nextPlatformType == PLATFORM_HELICOPTER; +} + bool checkMixerATRequired(mixerProfileATRequest_e required_action) { //return false if mixerAT condition is not required or setting is not valid @@ -132,17 +165,22 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) return false; } - if(currentMixerConfig.automated_switch){ - if ((required_action == MIXERAT_REQUEST_RTH) && STATE(MULTIROTOR)) - { - return true; - } - if ((required_action == MIXERAT_REQUEST_LAND) && STATE(AIRPLANE)) - { - return true; - } + switch (required_action) { + case MIXERAT_REQUEST_RTH: + return currentMixerConfig.automated_switch && STATE(MULTIROTOR) && platformTypeConfigured(PLATFORM_AIRPLANE); + + case MIXERAT_REQUEST_LAND: + return currentMixerConfig.automated_switch && STATE(AIRPLANE) && missionTransitionToMultirotorTypeConfigured(); + + case MIXERAT_REQUEST_MISSION_TO_FW: + return STATE(MULTIROTOR) && platformTypeConfigured(PLATFORM_AIRPLANE); + + case MIXERAT_REQUEST_MISSION_TO_MC: + return STATE(AIRPLANE) && missionTransitionToMultirotorTypeConfigured(); + + default: + return false; } - return false; } bool mixerATUpdateState(mixerProfileATRequest_e required_action) @@ -173,7 +211,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) break; case MIXERAT_PHASE_TRANSITIONING: isMixerTransitionMixing_requested = true; - if (millis() > mixerProfileAT.transitionTransEndTime){ + if (mixerATReadyForHotSwitch(required_action)){ isMixerTransitionMixing_requested = false; outputProfileHotSwitch(nextMixerProfileIndex); mixerProfileAT.phase = MIXERAT_PHASE_IDLE; diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 715732d0685..d44d65e67b8 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,6 +18,7 @@ typedef struct mixerConfig_s { bool controlProfileLinking; bool automated_switch; int16_t switchTransitionTimer; + uint16_t switchTransitionAirspeed; bool tailsitterOrientationOffset; int16_t transition_PID_mmix_multiplier_roll; int16_t transition_PID_mmix_multiplier_pitch; @@ -34,6 +35,8 @@ typedef enum { MIXERAT_REQUEST_NONE, //no request, stats checking only MIXERAT_REQUEST_RTH, MIXERAT_REQUEST_LAND, + MIXERAT_REQUEST_MISSION_TO_FW, + MIXERAT_REQUEST_MISSION_TO_MC, MIXERAT_REQUEST_ABORT, } mixerProfileATRequest_e; @@ -81,4 +84,4 @@ bool outputProfileHotSwitch(int profile_index); bool checkMixerProfileHotSwitchAvalibility(void); void activateMixerConfig(void); void mixerConfigInit(void); -void outputProfileUpdateTask(timeUs_t currentTimeUs); \ No newline at end of file +void outputProfileUpdateTask(timeUs_t currentTimeUs); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 8f60155b68c..bb824f81147 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -119,7 +119,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -149,6 +149,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter .waypoint_safe_distance = SETTING_NAV_WP_MAX_SAFE_DISTANCE_DEFAULT, // Metres - first waypoint should be closer than this + .vtol_mission_transition_user_action = SETTING_NAV_VTOL_MISSION_TRANSITION_USER_ACTION_DEFAULT, + .vtol_mission_transition_min_altitude = SETTING_NAV_VTOL_MISSION_TRANSITION_MIN_ALTITUDE_CM_DEFAULT, + .vtol_mission_transition_track_distance = SETTING_NAV_VTOL_MISSION_TRANSITION_TRACK_DISTANCE_CM_DEFAULT, #ifdef USE_MULTI_MISSION .waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT, // mission index selected from multi mission WP entry #endif @@ -270,6 +273,22 @@ uint16_t navEPV; int16_t navAccNEU[3]; //End of blackbox states +typedef struct navMixerATMissionTransition_s { + mixerProfileATRequest_e request; + int32_t heading; + bool active; +} navMixerATMissionTransition_t; + +typedef enum { + NAV_MISSION_VTOL_TRANSITION_NONE = 0, + NAV_MISSION_VTOL_TRANSITION_CONTINUE, + NAV_MISSION_VTOL_TRANSITION_START, + NAV_MISSION_VTOL_TRANSITION_REJECT, +} navMissionVtolTransitionDisposition_e; + +static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; +static navMixerATMissionTransition_t navMixerATMissionTransition; + static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode); static void updateDesiredRTHAltitude(void); static void resetAltitudeController(bool useTerrainFollowing); @@ -288,6 +307,7 @@ static void resetJumpCounter(void); static void clearJumpCounters(void); static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); +static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos); void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayPos(fpVector3_t * farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance); @@ -295,6 +315,9 @@ bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); static bool isWaypointMissionValid(void); +static void clearMissionVTOLTransitionState(void); +static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const navWaypoint_t *waypoint); +static void updateMissionTransitionGuidance(void); void missionPlannerSetWaypoint(void); void initializeRTHSanityChecker(void); @@ -1046,6 +1069,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT, [NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, //switch to its pending state [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, //switch to its pending state + [NAV_FSM_EVENT_MIXERAT_MISSION_RESUME] = NAV_STATE_WAYPOINT_IN_PROGRESS, } }, [NAV_STATE_MIXERAT_ABORT] = { @@ -1258,7 +1282,17 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) { - return navFSM[state].stateFlags; + navigationFSMStateFlags_t stateFlags = navFSM[state].stateFlags; + + // During mission-authorized MC->FW transition, enable XY/YAW control to fly a straight acceleration segment. + if ((state == NAV_STATE_MIXERAT_INITIALIZE || state == NAV_STATE_MIXERAT_IN_PROGRESS) && + navMixerATPendingState == NAV_STATE_WAYPOINT_PRE_ACTION && + navMixerATMissionTransition.active && + navMixerATMissionTransition.request == MIXERAT_REQUEST_MISSION_TO_FW) { + stateFlags |= NAV_CTL_POS | NAV_CTL_YAW; + } + + return stateFlags; } flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) @@ -1282,6 +1316,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState { UNUSED(previousState); + navMixerATPendingState = NAV_STATE_IDLE; + clearMissionVTOLTransitionState(); resetAltitudeController(false); resetHeadingController(); resetPositionController(); @@ -1965,6 +2001,110 @@ static navigationFSMEvent_t nextForNonGeoStates(void) } } +static uint16_t missionUserActionMask(const navMissionUserAction_e userAction) +{ + switch (userAction) { + case NAV_MISSION_USER_ACTION_1: + return NAV_WP_USER1; + case NAV_MISSION_USER_ACTION_2: + return NAV_WP_USER2; + case NAV_MISSION_USER_ACTION_3: + return NAV_WP_USER3; + case NAV_MISSION_USER_ACTION_4: + return NAV_WP_USER4; + default: + return 0; + } +} + +static bool isMissionTransitionToMultirotorType(const flyingPlatformType_e platformType) +{ + return platformType == PLATFORM_MULTIROTOR || + platformType == PLATFORM_TRICOPTER || + platformType == PLATFORM_HELICOPTER; +} + +static void clearMissionVTOLTransitionState(void) +{ + navMixerATMissionTransition.active = false; + navMixerATMissionTransition.request = MIXERAT_REQUEST_NONE; + navMixerATMissionTransition.heading = posControl.actualState.yaw; +} + +static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const navWaypoint_t *waypoint) +{ + const navMissionUserAction_e configuredUserAction = (navMissionUserAction_e)navConfig()->general.vtol_mission_transition_user_action; + const uint16_t configuredUserActionMask = missionUserActionMask(configuredUserAction); + + if (!configuredUserActionMask) { + return NAV_MISSION_VTOL_TRANSITION_NONE; + } + + // The configured USER action bit itself is the absolute target selector: + // 0 -> MC target, 1 -> FW target. + const bool transitionToFixedWing = ((((uint16_t)waypoint->p3) & configuredUserActionMask) != 0); + const mixerProfileATRequest_e requestedAction = transitionToFixedWing ? MIXERAT_REQUEST_MISSION_TO_FW : MIXERAT_REQUEST_MISSION_TO_MC; + + // Idempotent mission command: continue immediately if already in requested platform state. + if ((transitionToFixedWing && STATE(AIRPLANE)) || (!transitionToFixedWing && STATE(MULTIROTOR))) { + return NAV_MISSION_VTOL_TRANSITION_CONTINUE; + } + + if (!ARMING_FLAG(ARMED) || + FLIGHT_MODE(FAILSAFE_MODE) || + areSensorsCalibrating() || + posControl.flags.estPosStatus < EST_USABLE || + posControl.flags.estHeadingStatus < EST_USABLE || + !isModeActivationConditionPresent(BOXMIXERPROFILE) || + !checkMixerProfileHotSwitchAvalibility() || + mixerProfileAT.phase != MIXERAT_PHASE_IDLE) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + const uint16_t transitionMinAltitude = navConfig()->general.vtol_mission_transition_min_altitude; + if (transitionMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z < transitionMinAltitude) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + const flyingPlatformType_e targetPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; + if ((transitionToFixedWing && targetPlatformType != PLATFORM_AIRPLANE) || + (!transitionToFixedWing && !isMissionTransitionToMultirotorType(targetPlatformType))) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + if (!checkMixerATRequired(requestedAction)) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + int32_t transitionHeading = posControl.actualState.yaw; + if (transitionToFixedWing) { + fpVector3_t nextWpPos; + if (getLocalPosNextWaypoint(&nextWpPos)) { + transitionHeading = calculateBearingToDestination(&nextWpPos); + } + } + + navMixerATMissionTransition.request = requestedAction; + navMixerATMissionTransition.heading = wrap_36000(transitionHeading); + navMixerATMissionTransition.active = true; + return NAV_MISSION_VTOL_TRANSITION_START; +} + +static void updateMissionTransitionGuidance(void) +{ + if (navMixerATMissionTransition.active && + navMixerATMissionTransition.request == MIXERAT_REQUEST_MISSION_TO_FW && + STATE(MULTIROTOR)) { + fpVector3_t transitionTarget; + const uint32_t transitionTrackDistance = navConfig()->general.vtol_mission_transition_track_distance; + calculateFarAwayTarget(&transitionTarget, navMixerATMissionTransition.heading, transitionTrackDistance); + setDesiredPosition(&transitionTarget, navMixerATMissionTransition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return; + } + + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); +} + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState) { /* A helper function to do waypoint-specific action */ @@ -1973,12 +2113,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_HOLD_TIME: case NAV_WP_ACTION_WAYPOINT: - case NAV_WP_ACTION_LAND: - calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); + case NAV_WP_ACTION_LAND: { + const navWaypoint_t * const activeWaypoint = &posControl.waypointList[posControl.activeWaypointIndex]; + calculateAndSetActiveWaypoint(activeWaypoint); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; posControl.wpAltitudeReached = false; + + clearMissionVTOLTransitionState(); + const navMissionVtolTransitionDisposition_e transitionAction = prepareMissionVTOLTransition(activeWaypoint); + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_START) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_REJECT) { + return NAV_FSM_EVENT_ERROR; + } + return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS + } case NAV_WP_ACTION_JUMP: // We use p3 as the volatile jump counter (p2 is the static value) @@ -2284,7 +2436,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi return NAV_FSM_EVENT_NONE; } -navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState) { const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); @@ -2294,7 +2445,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navi resetAltitudeController(false); setupAltitudeController(); } - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + + if (previousState != NAV_STATE_WAYPOINT_PRE_ACTION) { + clearMissionVTOLTransitionState(); + } + + updateMissionTransitionGuidance(); navMixerATPendingState = previousState; return NAV_FSM_EVENT_SUCCESS; } @@ -2311,6 +2467,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav case NAV_STATE_RTH_LANDING: required_action = MIXERAT_REQUEST_LAND; break; + case NAV_STATE_WAYPOINT_PRE_ACTION: + required_action = navMixerATMissionTransition.active ? navMixerATMissionTransition.request : MIXERAT_REQUEST_NONE; + break; default: required_action = MIXERAT_REQUEST_NONE; break; @@ -2320,6 +2479,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL mixerATUpdateState(MIXERAT_REQUEST_ABORT); + const bool missionTransitionWasActive = navMixerATMissionTransition.active; + clearMissionVTOLTransitionState(); switch (navMixerATPendingState) { case NAV_STATE_RTH_HEAD_HOME: @@ -2330,13 +2491,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav setupAltitudeController(); return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; break; + case NAV_STATE_WAYPOINT_PRE_ACTION: + setupAltitudeController(); + return missionTransitionWasActive ? NAV_FSM_EVENT_MIXERAT_MISSION_RESUME : NAV_FSM_EVENT_SWITCH_TO_IDLE; + break; default: return NAV_FSM_EVENT_SWITCH_TO_IDLE; break; } } - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + updateMissionTransitionGuidance(); return NAV_FSM_EVENT_NONE; } @@ -2345,6 +2510,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigatio { UNUSED(previousState); mixerATUpdateState(MIXERAT_REQUEST_ABORT); + clearMissionVTOLTransitionState(); return NAV_FSM_EVENT_SUCCESS; } @@ -5062,6 +5228,8 @@ void navigationInit(void) { /* Initial state */ posControl.navState = NAV_STATE_IDLE; + navMixerATPendingState = NAV_STATE_IDLE; + clearMissionVTOLTransitionState(); posControl.flags.horizontalPositionDataNew = false; posControl.flags.verticalPositionDataNew = false; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 398781fa596..f8c009dcd07 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -328,6 +328,14 @@ typedef enum { WP_MISSION_SWITCH, } navMissionRestart_e; +typedef enum { + NAV_MISSION_USER_ACTION_OFF = 0, + NAV_MISSION_USER_ACTION_1, + NAV_MISSION_USER_ACTION_2, + NAV_MISSION_USER_ACTION_3, + NAV_MISSION_USER_ACTION_4, +} navMissionUserAction_e; + typedef enum { RTH_TRACKBACK_OFF, RTH_TRACKBACK_ON, @@ -413,6 +421,9 @@ typedef struct navConfig_s { uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance + uint8_t vtol_mission_transition_user_action; // User action slot that requests mission VTOL transition + uint16_t vtol_mission_transition_min_altitude; // Minimum altitude [cm] to start mission VTOL transition (0 = disabled) + uint32_t vtol_mission_transition_track_distance; // Straight-segment target distance [cm] used during MC->FW mission transition #ifdef USE_MULTI_MISSION uint8_t waypoint_multi_mission_index; // Index of mission to be loaded in multi mission entry #endif diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index a1f07e470c0..947bc3406ea 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -183,6 +183,7 @@ typedef enum { 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_LOITER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4, + NAV_FSM_EVENT_MIXERAT_MISSION_RESUME = NAV_FSM_EVENT_STATE_SPECIFIC_4, NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5, NAV_FSM_EVENT_COUNT,