From cd67c181f017312ee3c37948844151e9ea9fee3d Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 12 May 2026 17:00:23 +0300 Subject: [PATCH 01/42] 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, From 58b9fc7274d77aec9a2b797b3c0ae5ce59361211 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 12 May 2026 21:09:56 +0300 Subject: [PATCH 02/42] implement VTOL transition automation for smoothing transition mission WP auto transmision available --- docs/MixerProfile.md | 45 ++++- docs/Navigation.md | 6 +- src/main/fc/settings.yaml | 46 +++++ src/main/flight/mixer.c | 24 ++- src/main/flight/mixer_profile.c | 327 ++++++++++++++++++++++++++++--- src/main/flight/mixer_profile.h | 34 ++++ src/main/flight/servos.c | 3 +- src/main/navigation/navigation.c | 8 +- 8 files changed, 455 insertions(+), 38 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 1fdd9132dad..d8688ec5d85 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -22,6 +22,17 @@ Transition input is disabled when navigation mode is activate The use of Transition Mode is recommended to enable further features and future developments like fail-safe support. Mapping motor to servo output, or servo with logic conditions is **not** recommended +`MIXER TRANSITION` now behaves as a transition trigger/request (edge-triggered), not a continuous blend hold: + +- A rising edge starts one transition (MC->FW or FW->MC depending on current profile). +- The transition state machine runs automatically to completion. +- Keeping the mode ON does not repeatedly restart transitions. +- A new transition requires mode OFF then ON again. +- If switched OFF before hot-switch completes, the manual transition request is aborted. + +This edge-triggered behavior is enabled by `manual_vtol_transition_controller`. +When `manual_vtol_transition_controller = OFF`, manual transition keeps legacy behavior. + ## Servo `Mixer Transition` is the input source for transition input; use this to tilt motor to gain airspeed. @@ -72,13 +83,43 @@ 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. +### Unified VTOL transition controller + +Manual `MIXER TRANSITION` and mission-authorized VTOL transition both use the same internal transition controller. +This controller computes transition progress, controls mixer transition scaling, and performs profile hot-switch only inside the authorized transition state. + +### Airspeed-first completion + +When pitot airspeed is healthy and available, transition completion uses pitot thresholds: + +- `vtol_transition_to_fw_min_airspeed_cm_s` for MC->FW +- `vtol_transition_to_mc_max_airspeed_cm_s` for FW->MC + +If pitot is unavailable/unhealthy (or threshold is `0`), timer fallback is used (`mixer_switch_trans_timer`). +Ground speed is not used for transition completion/progress. + +Optional safety timeout: + +- `vtol_transition_airspeed_timeout_ms` can abort transition if airspeed condition is not met in time. + +### Dynamic scaling (optional) + +When `vtol_transition_dynamic_mixer = ON`, transition progress scales: + +- pusher contribution (`-2.0 < throttle < -1.0` motors) from configured max toward 0/100% depending on direction, +- lift motor throttle contribution (`vtol_transition_lift_end_percent`), +- MC stabilization authority (`vtol_transition_mc_authority_end_percent`), +- FW authority start level (`vtol_transition_fw_authority_start_percent`, servo transition input blend). + +Default is OFF to preserve existing behavior. + ### 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) +- `mixer_switch_trans_airspeed_cm_s` (legacy MC->FW threshold fallback/compatibility) On each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`), the configured USER action bit is used as absolute target selector: @@ -90,7 +131,7 @@ On each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`), the con 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`. +Mission path uses the same controller and completion logic as manual transition (airspeed-first, timer fallback). 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. diff --git a/docs/Navigation.md b/docs/Navigation.md index c716925ba77..02000d0e487 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -111,7 +111,7 @@ 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. +- VTOL transition completion logic is shared with manual MIXER TRANSITION and uses the mixer transition settings. Behavior on each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`): @@ -125,7 +125,9 @@ Behavior on each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`) 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. +- MC -> FW and FW -> MC completion uses pitot airspeed thresholds when healthy/available (`vtol_transition_to_fw_min_airspeed_cm_s`, `vtol_transition_to_mc_max_airspeed_cm_s`). +- If pitot is unavailable/unhealthy (or threshold disabled), timer fallback (`mixer_switch_trans_timer`) is used. +- Ground speed is not used for transition progress/completion. - 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. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 44e34452fee..68b058e8d37 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1286,6 +1286,52 @@ groups: field: mixer_config.switchTransitionAirspeed min: 0 max: 10000 + - name: vtol_transition_dynamic_mixer + description: "Enables dynamic VTOL transition progress/scaling controller shared by mission-authorized and manual MIXER TRANSITION paths." + default_value: OFF + field: mixer_config.vtolTransitionDynamicMixer + type: bool + - name: manual_vtol_transition_controller + description: "Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior." + default_value: OFF + field: mixer_config.manualVtolTransitionController + type: bool + - name: vtol_transition_to_fw_min_airspeed_cm_s + description: "Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. If 0, `mixer_switch_trans_airspeed_cm_s` is used for MC->FW." + default_value: 0 + field: mixer_config.vtolTransitionToFwMinAirspeed + min: 0 + max: 20000 + - name: vtol_transition_to_mc_max_airspeed_cm_s + description: "Maximum pitot airspeed [cm/s] allowed to complete FW->MC transition when airspeed is healthy and available. If 0, FW->MC uses timer fallback." + default_value: 0 + field: mixer_config.vtolTransitionToMcMaxAirspeed + min: 0 + max: 20000 + - name: vtol_transition_airspeed_timeout_ms + description: "Safety timeout [ms] for airspeed-controlled transitions. If non-zero and required airspeed condition is not met in time, transition aborts instead of force-completing." + default_value: 0 + field: mixer_config.vtolTransitionAirspeedTimeoutMs + min: 0 + max: 60000 + - name: vtol_transition_lift_end_percent + description: "Target vertical-lift throttle scale at transition end, in percent. Used only when `vtol_transition_dynamic_mixer` is ON." + default_value: 100 + field: mixer_config.vtolTransitionLiftEndPercent + min: 0 + max: 100 + - name: vtol_transition_mc_authority_end_percent + description: "Target multicopter stabilization authority scale at transition end, in percent. Used only when `vtol_transition_dynamic_mixer` is ON." + default_value: 100 + field: mixer_config.vtolTransitionMcAuthorityEndPercent + min: 0 + max: 100 + - name: vtol_transition_fw_authority_start_percent + description: "Initial fixed-wing authority scale at transition start, in percent. Used only when `vtol_transition_dynamic_mixer` is ON." + default_value: 100 + field: mixer_config.vtolTransitionFwAuthorityStartPercent + min: 0 + max: 100 - name: tailsitter_orientation_offset description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode" default_value: OFF diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80992b772d..030d56cc0ca 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -48,6 +48,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" @@ -520,10 +521,11 @@ void FAST_CODE mixTable(void) input[ROLL] = axisPID[ROLL]; input[PITCH] = axisPID[PITCH]; input[YAW] = axisPID[YAW]; - if(isMixerTransitionMixing){ - input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f); - input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f); - input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f); + if (isMixerTransitionMixing) { + const float mcAuthorityScale = mixerATGetMcAuthorityScale(); + input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f) * mcAuthorityScale; + input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f) * mcAuthorityScale; + input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f) * mcAuthorityScale; } } @@ -624,8 +626,16 @@ void FAST_CODE mixTable(void) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. + const float liftScale = isMixerTransitionMixing ? mixerATGetLiftScale() : 1.0f; + const float pusherScale = isMixerTransitionMixing ? mixerATGetPusherScale() : 1.0f; + for (int i = 0; i < motorCount; i++) { - motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax); + float motorThrottle = mixerThrottleCommand * currentMixer[i].throttle; + if (currentMixer[i].throttle > 0.0f) { + motorThrottle *= liftScale; + } + + motor[i] = rpyMix[i] + constrain(motorThrottle, throttleMin, throttleMax); if (failsafeIsActive()) { motor[i] = constrain(motor[i], motorConfig()->mincommand, getMaxThrottle()); @@ -639,7 +649,7 @@ void FAST_CODE mixTable(void) } //spin stopped motors only in mixer transition mode if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && !feature(FEATURE_REVERSIBLE_MOTORS)) { - motor[i] = -currentMixer[i].throttle * 1000; + motor[i] = -currentMixer[i].throttle * 1000 * pusherScale; motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } } @@ -737,4 +747,4 @@ uint16_t getMaxThrottle(void) { } return throttle; -} \ No newline at end of file +} diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 4362f418476..208b0664d68 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -10,6 +10,7 @@ #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" #include "drivers/time.h" +#include "common/maths.h" #include "flight/mixer.h" #include "common/axis.h" #include "flight/pid.h" @@ -18,6 +19,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" #include "sensors/pitotmeter.h" +#include "sensors/sensors.h" #include "fc/fc_core.h" #include "fc/config.h" @@ -37,6 +39,8 @@ bool isMixerTransitionMixing; bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; int nextMixerProfileIndex; +static bool manualTransitionModeWasActive; +static bool manualTransitionReadyForEdge = true; PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 2); @@ -55,6 +59,14 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, .switchTransitionAirspeed = SETTING_MIXER_SWITCH_TRANS_AIRSPEED_CM_S_DEFAULT, + .vtolTransitionDynamicMixer = SETTING_VTOL_TRANSITION_DYNAMIC_MIXER_DEFAULT, + .manualVtolTransitionController = SETTING_MANUAL_VTOL_TRANSITION_CONTROLLER_DEFAULT, + .vtolTransitionToFwMinAirspeed = SETTING_VTOL_TRANSITION_TO_FW_MIN_AIRSPEED_CM_S_DEFAULT, + .vtolTransitionToMcMaxAirspeed = SETTING_VTOL_TRANSITION_TO_MC_MAX_AIRSPEED_CM_S_DEFAULT, + .vtolTransitionAirspeedTimeoutMs = SETTING_VTOL_TRANSITION_AIRSPEED_TIMEOUT_MS_DEFAULT, + .vtolTransitionLiftEndPercent = SETTING_VTOL_TRANSITION_LIFT_END_PERCENT_DEFAULT, + .vtolTransitionMcAuthorityEndPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_END_PERCENT_DEFAULT, + .vtolTransitionFwAuthorityStartPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_START_PERCENT_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, @@ -110,27 +122,171 @@ void mixerConfigInit(void) void setMixerProfileAT(void) { - mixerProfileAT.transitionStartTime = millis(); - mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; + const timeMs_t now = millis(); + const uint32_t transitionDurationMs = MAX(0, currentMixerConfig.switchTransitionTimer) * 100; + + mixerProfileAT.transitionStartTime = now; + mixerProfileAT.transitionStabEndTime = now; + mixerProfileAT.transitionTransEndTime = now + transitionDurationMs; + mixerProfileAT.aborted = false; + mixerProfileAT.hotSwitchDone = false; + mixerProfileAT.usedAirspeed = false; + mixerProfileAT.progress = 0.0f; + mixerProfileAT.blendToFw = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW ? 0.0f : 1.0f; + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; } static bool requestTransitionsToFixedWing(const mixerProfileATRequest_e required_action) { - return required_action == MIXERAT_REQUEST_RTH || required_action == MIXERAT_REQUEST_MISSION_TO_FW; + return required_action == MIXERAT_REQUEST_RTH || + required_action == MIXERAT_REQUEST_MISSION_TO_FW || + required_action == MIXERAT_REQUEST_MANUAL_TO_FW; } -static bool mixerATReadyForHotSwitch(const mixerProfileATRequest_e required_action) +static mixerProfileATDirection_e directionForRequest(const mixerProfileATRequest_e required_action) +{ + if (requestTransitionsToFixedWing(required_action)) { + return MIXERAT_DIRECTION_TO_FW; + } + + if (required_action == MIXERAT_REQUEST_LAND || + required_action == MIXERAT_REQUEST_MISSION_TO_MC || + required_action == MIXERAT_REQUEST_MANUAL_TO_MC) { + return MIXERAT_DIRECTION_TO_MC; + } + + return MIXERAT_DIRECTION_NONE; +} + +static void resetTransitionScales(void) +{ + mixerProfileAT.progress = 0.0f; + mixerProfileAT.blendToFw = 0.0f; + mixerProfileAT.pusherScale = 0.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; +} + +static float blendScale(float from, float to, float progress) +{ + return from + (to - from) * constrainf(progress, 0.0f, 1.0f); +} + +static bool hasTrustedPitotAirspeed(float *airspeedCmS) { #ifdef USE_PITOT - if (requestTransitionsToFixedWing(required_action) && - currentMixerConfig.switchTransitionAirspeed > 0 && - pitotValidForAirspeed()) { - return getAirspeedEstimate() >= currentMixerConfig.switchTransitionAirspeed; + if (!sensors(SENSOR_PITOT) || !pitotValidForAirspeed() || pitotHasFailed()) { + return false; } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE || + detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + return false; + } + + *airspeedCmS = pitot.airSpeed; + return true; +#else + UNUSED(airspeedCmS); + return false; #endif +} + +static uint16_t getAirspeedThresholdForDirection(const mixerProfileATDirection_e direction) +{ + if (direction == MIXERAT_DIRECTION_TO_FW) { + if (currentMixerConfig.vtolTransitionToFwMinAirspeed > 0) { + return currentMixerConfig.vtolTransitionToFwMinAirspeed; + } + return currentMixerConfig.switchTransitionAirspeed; + } - // Timer remains a fallback when airspeed is not configured/available. - return millis() > mixerProfileAT.transitionTransEndTime; + if (direction == MIXERAT_DIRECTION_TO_MC) { + return currentMixerConfig.vtolTransitionToMcMaxAirspeed; + } + + return 0; +} + +static void updateTransitionScales(void) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer) { + mixerProfileAT.blendToFw = 1.0f; + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; + return; + } + + const float liftFloor = constrainf(currentMixerConfig.vtolTransitionLiftEndPercent / 100.0f, 0.0f, 1.0f); + const float mcFloor = constrainf(currentMixerConfig.vtolTransitionMcAuthorityEndPercent / 100.0f, 0.0f, 1.0f); + const float fwFloor = constrainf(currentMixerConfig.vtolTransitionFwAuthorityStartPercent / 100.0f, 0.0f, 1.0f); + + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, mixerProfileAT.progress); + mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, mixerProfileAT.progress); + mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, mixerProfileAT.progress); + mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, mixerProfileAT.progress); + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, mixerProfileAT.progress); + mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, mixerProfileAT.progress); + mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, mixerProfileAT.progress); + mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, mixerProfileAT.progress); + } + + mixerProfileAT.blendToFw = constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); +} + +static void abortTransition(void) +{ + const bool wasActive = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; + isMixerTransitionMixing_requested = false; + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.aborted = wasActive; + mixerProfileAT.hotSwitchDone = false; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + resetTransitionScales(); +} + +static bool mixerATReadyForHotSwitch(const mixerProfileATRequest_e required_action) +{ + const mixerProfileATDirection_e direction = directionForRequest(required_action); + const uint16_t airspeedThresholdCmS = getAirspeedThresholdForDirection(direction); + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + const uint32_t transitionTimerMs = MAX(0, currentMixerConfig.switchTransitionTimer) * 100; + float airspeedCmS = 0.0f; + + if (direction == MIXERAT_DIRECTION_NONE) { + mixerProfileAT.progress = 0.0f; + mixerProfileAT.usedAirspeed = false; + return false; + } + + if (airspeedThresholdCmS > 0 && hasTrustedPitotAirspeed(&airspeedCmS)) { + mixerProfileAT.usedAirspeed = true; + if (direction == MIXERAT_DIRECTION_TO_FW) { + mixerProfileAT.progress = constrainf(airspeedCmS / airspeedThresholdCmS, 0.0f, 1.0f); + return airspeedCmS >= airspeedThresholdCmS; + } + + mixerProfileAT.progress = constrainf((airspeedThresholdCmS - airspeedCmS) / airspeedThresholdCmS, 0.0f, 1.0f); + return airspeedCmS <= airspeedThresholdCmS; + } + + mixerProfileAT.usedAirspeed = false; + if (transitionTimerMs > 0) { + mixerProfileAT.progress = constrainf((float)elapsedMs / (float)transitionTimerMs, 0.0f, 1.0f); + } else { + mixerProfileAT.progress = 1.0f; + } + + return elapsedMs >= transitionTimerMs; } bool platformTypeConfigured(flyingPlatformType_e platformType) @@ -178,6 +334,12 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) case MIXERAT_REQUEST_MISSION_TO_MC: return STATE(AIRPLANE) && missionTransitionToMultirotorTypeConfigured(); + case MIXERAT_REQUEST_MANUAL_TO_FW: + return STATE(MULTIROTOR) && platformTypeConfigured(PLATFORM_AIRPLANE); + + case MIXERAT_REQUEST_MANUAL_TO_MC: + return STATE(AIRPLANE) && missionTransitionToMultirotorTypeConfigured(); + default: return false; } @@ -190,34 +352,57 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) do { reprocessState=false; - if (required_action==MIXERAT_REQUEST_ABORT){ - isMixerTransitionMixing_requested = false; - mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + if (required_action == MIXERAT_REQUEST_ABORT) { + abortTransition(); return true; } - switch (mixerProfileAT.phase){ + switch (mixerProfileAT.phase) { case MIXERAT_PHASE_IDLE: //check if mixerAT is required - if (checkMixerATRequired(required_action)){ - mixerProfileAT.phase=MIXERAT_PHASE_TRANSITION_INITIALIZE; + if (checkMixerATRequired(required_action)) { + mixerProfileAT.request = required_action; + mixerProfileAT.direction = directionForRequest(required_action); + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITION_INITIALIZE; reprocessState = true; + } else { + resetTransitionScales(); } break; case MIXERAT_PHASE_TRANSITION_INITIALIZE: - // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); + mixerProfileAT.request = required_action; + mixerProfileAT.direction = directionForRequest(required_action); setMixerProfileAT(); mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; reprocessState = true; break; case MIXERAT_PHASE_TRANSITIONING: isMixerTransitionMixing_requested = true; - if (mixerATReadyForHotSwitch(required_action)){ + if (required_action != MIXERAT_REQUEST_NONE && required_action != mixerProfileAT.request) { + abortTransition(); + return true; + } + + if (mixerATReadyForHotSwitch(mixerProfileAT.request)) { isMixerTransitionMixing_requested = false; - outputProfileHotSwitch(nextMixerProfileIndex); + if (!outputProfileHotSwitch(nextMixerProfileIndex)) { + abortTransition(); + return true; + } + mixerProfileAT.hotSwitchDone = true; + mixerProfileAT.progress = 1.0f; + updateTransitionScales(); mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; reprocessState = true; - //transition is done + } else if (mixerProfileAT.usedAirspeed && + currentMixerConfig.vtolTransitionAirspeedTimeoutMs > 0 && + (millis() - mixerProfileAT.transitionStartTime) >= currentMixerConfig.vtolTransitionAirspeedTimeoutMs) { + abortTransition(); + return true; } + + updateTransitionScales(); return false; break; default: @@ -240,17 +425,111 @@ bool checkMixerProfileHotSwitchAvalibility(void) void outputProfileUpdateTask(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - if(cliMode) return; - bool mixerAT_inuse = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; + if (cliMode) { + return; + } + + bool mixerAT_inuse = mixerATIsActive(); + const bool transitionModeActive = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); + const bool transitionModeRisingEdge = transitionModeActive && !manualTransitionModeWasActive; + const bool manualTransitionAllowed = (posControl.navState == NAV_STATE_IDLE) || + (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS); + const bool missionActive = (navGetCurrentStateFlags() & NAV_AUTO_WP) != 0; + const bool manualControllerEnabled = currentMixerConfig.manualVtolTransitionController && !missionActive; + + if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { + abortTransition(); + mixerAT_inuse = false; + } + // transition mode input for servo mix and motor mix if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) { if (isModeActivationConditionPresent(BOXMIXERPROFILE)){ outputProfileHotSwitch(IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1); } - isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); } - isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); + + if (!manualControllerEnabled) { + // Backward-compatible manual path: level-controlled transition mixing request. + if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) { + isMixerTransitionMixing_requested = transitionModeActive; + } + manualTransitionReadyForEdge = true; + } else { + if (!transitionModeActive) { + manualTransitionReadyForEdge = true; + if (!mixerAT_inuse) { + isMixerTransitionMixing_requested = false; + } + } else if (transitionModeRisingEdge && manualTransitionReadyForEdge && manualTransitionAllowed && !mixerAT_inuse) { + manualTransitionReadyForEdge = false; + if (STATE(MULTIROTOR)) { + mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_FW); + } else if (STATE(AIRPLANE)) { + mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_MC); + } + mixerAT_inuse = mixerATIsActive(); + } + + if (!transitionModeActive && + mixerAT_inuse && + !mixerProfileAT.hotSwitchDone && + (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { + abortTransition(); + mixerAT_inuse = false; + } + + if (mixerAT_inuse && + (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { + mixerATUpdateState(mixerProfileAT.request); + mixerAT_inuse = mixerATIsActive(); + } + } + + manualTransitionModeWasActive = transitionModeActive; + + isMixerTransitionMixing = isMixerTransitionMixing_requested && + ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); + + if (!isMixerTransitionMixing) { + resetTransitionScales(); + } +} + +bool mixerATIsActive(void) +{ + return mixerProfileAT.phase != MIXERAT_PHASE_IDLE; +} + +bool mixerATWasAborted(void) +{ + return mixerProfileAT.aborted; +} + +float mixerATGetPusherScale(void) +{ + return constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f); +} + +float mixerATGetLiftScale(void) +{ + return constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f); +} + +float mixerATGetMcAuthorityScale(void) +{ + return constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f); +} + +float mixerATGetFwAuthorityScale(void) +{ + return constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); +} + +float mixerATGetBlendToFw(void) +{ + return constrainf(mixerProfileAT.blendToFw, 0.0f, 1.0f); } // switch mixerprofile without reboot diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index d44d65e67b8..1b3ae5f6f86 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -19,6 +19,14 @@ typedef struct mixerConfig_s { bool automated_switch; int16_t switchTransitionTimer; uint16_t switchTransitionAirspeed; + bool vtolTransitionDynamicMixer; + bool manualVtolTransitionController; + uint16_t vtolTransitionToFwMinAirspeed; + uint16_t vtolTransitionToMcMaxAirspeed; + uint16_t vtolTransitionAirspeedTimeoutMs; + uint8_t vtolTransitionLiftEndPercent; + uint8_t vtolTransitionMcAuthorityEndPercent; + uint8_t vtolTransitionFwAuthorityStartPercent; bool tailsitterOrientationOffset; int16_t transition_PID_mmix_multiplier_roll; int16_t transition_PID_mmix_multiplier_pitch; @@ -37,9 +45,17 @@ typedef enum { MIXERAT_REQUEST_LAND, MIXERAT_REQUEST_MISSION_TO_FW, MIXERAT_REQUEST_MISSION_TO_MC, + MIXERAT_REQUEST_MANUAL_TO_FW, + MIXERAT_REQUEST_MANUAL_TO_MC, MIXERAT_REQUEST_ABORT, } mixerProfileATRequest_e; +typedef enum { + MIXERAT_DIRECTION_NONE = 0, + MIXERAT_DIRECTION_TO_FW, + MIXERAT_DIRECTION_TO_MC, +} mixerProfileATDirection_e; + //mixerProfile Automated Transition PHASE typedef enum { MIXERAT_PHASE_IDLE, @@ -50,7 +66,18 @@ typedef enum { typedef struct mixerProfileAT_s { mixerProfileATState_e phase; + mixerProfileATDirection_e direction; + mixerProfileATRequest_e request; bool transitionInputMixing; + bool aborted; + bool hotSwitchDone; + bool usedAirspeed; + float progress; + float blendToFw; + float pusherScale; + float liftScale; + float mcAuthorityScale; + float fwAuthorityScale; timeMs_t transitionStartTime; timeMs_t transitionStabEndTime; timeMs_t transitionTransEndTime; @@ -58,6 +85,13 @@ typedef struct mixerProfileAT_s { extern mixerProfileAT_t mixerProfileAT; bool checkMixerATRequired(mixerProfileATRequest_e required_action); bool mixerATUpdateState(mixerProfileATRequest_e required_action); +bool mixerATIsActive(void); +bool mixerATWasAborted(void); +float mixerATGetPusherScale(void); +float mixerATGetLiftScale(void); +float mixerATGetMcAuthorityScale(void); +float mixerATGetFwAuthorityScale(void); +float mixerATGetBlendToFw(void); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 9f6eb4851a7..8c9276a1fbc 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -51,6 +51,7 @@ #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" @@ -353,7 +354,7 @@ void servoMixer(float dT) input[INPUT_STABILIZED_THROTTLE] = mixerThrottleCommand - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] - input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing * 500; //fixed value + input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing ? lrintf(mixerATGetBlendToFw() * 500.0f) : 0; input[INPUT_MIXER_SWITCH_HELPER] = 0; // no input, used to apply speed limit filter from previous servo rules // center the RC input value around the RC middle value diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bb824f81147..c78f7d8964d 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2057,7 +2057,7 @@ static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const posControl.flags.estHeadingStatus < EST_USABLE || !isModeActivationConditionPresent(BOXMIXERPROFILE) || !checkMixerProfileHotSwitchAvalibility() || - mixerProfileAT.phase != MIXERAT_PHASE_IDLE) { + mixerATIsActive()) { return NAV_MISSION_VTOL_TRANSITION_REJECT; } @@ -2476,6 +2476,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav } if (mixerATUpdateState(required_action)){ // MixerAT is done, switch to next state + const bool transitionAborted = mixerATWasAborted(); resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL mixerATUpdateState(MIXERAT_REQUEST_ABORT); @@ -2493,7 +2494,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav break; case NAV_STATE_WAYPOINT_PRE_ACTION: setupAltitudeController(); - return missionTransitionWasActive ? NAV_FSM_EVENT_MIXERAT_MISSION_RESUME : NAV_FSM_EVENT_SWITCH_TO_IDLE; + if (missionTransitionWasActive) { + return transitionAborted ? NAV_FSM_EVENT_SWITCH_TO_IDLE : NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; + } + return NAV_FSM_EVENT_SWITCH_TO_IDLE; break; default: return NAV_FSM_EVENT_SWITCH_TO_IDLE; From ae15cb97043d80f0892b2742003bf8ae62affd04 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 12 May 2026 23:01:09 +0300 Subject: [PATCH 03/42] feat(vtol): add unified manual/mission transition controller with airspeed-first logic and dynamic scaling - Introduce a common VTOL transition controller path used by: - manual MIXER TRANSITION (edge-triggered mode, optional via manual_vtol_transition_controller) - mission-authorized VTOL transition via nav_vtol_mission_transition_user_action - Keep profile hot-switch safety boundaries intact: - no broad manual mixer switching in active waypoint navigation - switching remains authorized only through transition state handling - Add airspeed-first completion behavior: - MC->FW threshold via vtol_transition_to_fw_min_airspeed_cm_s - FW->MC threshold via vtol_transition_to_mc_max_airspeed_cm_s - timer fallback only when pitot is unavailable/unhealthy - timeout/abort support via vtol_transition_airspeed_timeout_ms - Add optional dynamic mixer scaling (vtol_transition_dynamic_mixer): - pusher contribution ramping - lift throttle scaling (vtol_transition_lift_end_percent) - MC authority scaling (vtol_transition_mc_authority_end_percent) - FW authority blend scaling (vtol_transition_fw_authority_start_percent) - Fix transition scaling/progress details: - pusher ramp uses idle-to-target interpolation - FW->MC progress uses captured transition start airspeed for smooth deceleration-based ramp - Improve transition abort/reset robustness: - clear transition/nav mission transition state on disarm/failsafe/abort paths - avoid blind mission resume after half-complete transition - Add mission VTOL settings and behavior: - nav_vtol_mission_transition_user_action - nav_vtol_mission_transition_min_altitude_cm - nav_vtol_mission_transition_track_distance_cm - mission pause/resume around transition, straight-line MC->FW transition segment - Update documentation: - MixerProfile.md, Navigation.md, VTOL.md - document unified controller, manual semantics, mission semantics, airspeed precedence, dynamic scaling, and CLI usage --- docs/MixerProfile.md | 21 ++++- docs/Navigation.md | 6 +- docs/VTOL.md | 147 ++++++++++++++++++++++++++++++- src/main/fc/settings.yaml | 4 +- src/main/flight/mixer.c | 4 +- src/main/flight/mixer_profile.c | 31 ++++++- src/main/flight/mixer_profile.h | 2 + src/main/navigation/navigation.c | 7 ++ 8 files changed, 214 insertions(+), 8 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index d8688ec5d85..64f9bc7a3c1 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -86,7 +86,8 @@ When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Mod ### Unified VTOL transition controller Manual `MIXER TRANSITION` and mission-authorized VTOL transition both use the same internal transition controller. -This controller computes transition progress, controls mixer transition scaling, and performs profile hot-switch only inside the authorized transition state. +This controller always computes transition progress/completion and performs profile hot-switch only inside the authorized transition state. +When `vtol_transition_dynamic_mixer = ON`, that progress is also used for pusher/lift/authority scaling. ### Airspeed-first completion @@ -94,6 +95,7 @@ When pitot airspeed is healthy and available, transition completion uses pitot t - `vtol_transition_to_fw_min_airspeed_cm_s` for MC->FW - `vtol_transition_to_mc_max_airspeed_cm_s` for FW->MC +- If `vtol_transition_to_fw_min_airspeed_cm_s = 0`, MC->FW falls back to legacy `mixer_switch_trans_airspeed_cm_s`. If pitot is unavailable/unhealthy (or threshold is `0`), timer fallback is used (`mixer_switch_trans_timer`). Ground speed is not used for transition completion/progress. @@ -112,6 +114,7 @@ When `vtol_transition_dynamic_mixer = ON`, transition progress scales: - FW authority start level (`vtol_transition_fw_authority_start_percent`, servo transition input blend). Default is OFF to preserve existing behavior. +With dynamic scaling enabled, `vtol_transition_fw_authority_start_percent = 100` preserves legacy FW authority handoff; lower values provide smoother ramp-in. ### Mission-authorized VTOL transition (waypoint User Action) @@ -119,12 +122,14 @@ INAV supports mission-requested VTOL transitions through the existing automated - `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` (legacy MC->FW threshold fallback/compatibility) +- `vtol_transition_to_fw_min_airspeed_cm_s` (preferred MC->FW threshold) +- `mixer_switch_trans_airspeed_cm_s` (legacy MC->FW fallback when preferred threshold is `0`) 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 +- When `nav_vtol_mission_transition_user_action != OFF`, each navigable waypoint encodes a target state via that selected bit. - This is **not** a toggle command. - If already in the requested profile type, the action is treated as complete (idempotent). @@ -135,6 +140,18 @@ Mission path uses the same controller and completion logic as manual transition 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. +### Validation Matrix (PR / SITL / HITL) + +- MC->FW manual, pitot healthy/available. +- MC->FW manual, no pitot (timer fallback). +- FW->MC manual, pitot healthy/available. +- FW->MC manual, no pitot (timer fallback). +- `MIXER TRANSITION` held ON after completion (no repeated starts). +- `MIXER TRANSITION` OFF before hot-switch (safe abort). +- Mission transition with selected USER bit = `1` (TO_FW). +- Mission transition with selected USER bit = `0` (TO_MC). +- Failsafe/disarm during active transition (abort and no blind mission resume). + ## 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 02000d0e487..730ce10f7d4 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -111,13 +111,17 @@ 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. -- VTOL transition completion logic is shared with manual MIXER TRANSITION and uses the mixer transition settings. +- VTOL transition completion logic is shared with manual MIXER TRANSITION and uses mixer transition settings: + - preferred MC->FW threshold: `vtol_transition_to_fw_min_airspeed_cm_s` + - legacy MC->FW fallback (when preferred threshold is `0`): `mixer_switch_trans_airspeed_cm_s` + - FW->MC threshold: `vtol_transition_to_mc_max_airspeed_cm_s` 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 +- When `nav_vtol_mission_transition_user_action != OFF`, each navigable waypoint always encodes target state via that selected USER bit. - 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. diff --git a/docs/VTOL.md b/docs/VTOL.md index 8341c81086d..5efcd250951 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -239,7 +239,7 @@ If you have set up the mixer as suggested in STEP1 and STEP2, you may have to de # STEP 5: Transition Mixing (Multi-Rotor Profile)(Recommended) ### Transition Mixing is typically useful in multi-copter profile to gain airspeed in prior to entering the fixed-wing profile. When the `MIXER TRANSITION` mode is activated, the associated motor or servo will move according to your configured Transition Mixing. -Please note that transition input is disabled when a navigation mode is activated. The use of Transition Mixing is necessary to enable additional features such as VTOL RTH with out stalling. +Please note that manual transition input is disabled when a navigation mode is active. Mission-authorized VTOL transition (via configured waypoint User Action) still works through the automated transition state. ## Servo 'Transition Mixing': Tilting rotor configuration. Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weight/rate according to your desired angle. This will allow tilting the motor for tilting rotor model. @@ -283,6 +283,151 @@ set mixer_automated_switch = ON If you set `mixer_automated_switch` to `OFF` for all mixer profiles (the default setting), the model will not perform automated transitions. You can always enable navigation modes after performing a manual transition. +## Unified VTOL Transition Controller (Manual + Mission) + +INAV now uses one internal VTOL transition controller for both: +- manual `MIXER TRANSITION` requests, and +- mission-authorized VTOL transitions. + +This keeps one safety boundary for profile hot-switching and avoids separate transition implementations. + +### Behavior summary + +- Transition progress is always computed internally. +- Pitot airspeed is the primary source for transition completion when healthy/available. +- Timer is used as fallback when pitot is unavailable/unhealthy. +- Ground speed is not used for transition completion. +- Mission transition uses the same controller and does not directly manipulate motors. +- Manual `MIXER PROFILE` / `MIXER TRANSITION` bypass during normal waypoint navigation is still blocked. + +### Manual transition semantics + +With `manual_vtol_transition_controller = ON`: +- `MIXER TRANSITION` acts as an edge-triggered request. +- A rising edge starts one transition. +- Transition then runs autonomously to completion. +- Keeping the mode ON does not repeatedly retrigger transition. +- To start another transition, mode must go OFF then ON again. +- If mode is turned OFF before hot-switch, transition request is aborted safely. + +With `manual_vtol_transition_controller = OFF`: +- legacy manual behavior is preserved for backward compatibility. + +### Mission-authorized transition semantics + +Mission transition is configured with `nav_vtol_mission_transition_user_action`. + +- `OFF`: feature disabled. +- `USER1`..`USER4`: selected User Action bit is used as target selector on navigable waypoints. +- selected bit `0` -> target MC profile +- selected bit `1` -> target FW profile +- Mission progression pauses during transition and resumes only after completion. +- If already in requested target profile, command is idempotent (no new transition). + +For MC -> FW mission transition: +- guidance uses a straight acceleration segment (no loiter), +- normal waypoint advancement is paused during transition. + +### Airspeed-first completion logic + +MC -> FW: +- completion threshold: `vtol_transition_to_fw_min_airspeed_cm_s` +- if this is `0`, legacy `mixer_switch_trans_airspeed_cm_s` is used. + +FW -> MC: +- completion threshold: `vtol_transition_to_mc_max_airspeed_cm_s` + +Timeout: +- `vtol_transition_airspeed_timeout_ms` can abort transition if condition is not achieved in time. + +### Dynamic mixer scaling + +When `vtol_transition_dynamic_mixer = ON`, transition progress additionally scales: +- pusher transition contribution, +- vertical lift contribution, +- MC stabilization authority, +- FW transition input authority blend. + +When `vtol_transition_dynamic_mixer = OFF`, legacy static transition mixing behavior is preserved. + +### Detailed effect of the three percentage settings + +These three settings are active only when `vtol_transition_dynamic_mixer = ON`. + +1. `vtol_transition_lift_end_percent` +- Defines lift throttle scale at transition end. +- MC -> FW: lift goes from `100%` at start to `lift_end_percent` at end. +- FW -> MC: lift goes from `lift_end_percent` at start to `100%` at end. + +Example (`vtol_transition_lift_end_percent = 20`): +- MC -> FW at 50% progress: lift scale is about 60%. +- FW -> MC at 50% progress: lift scale is about 60%. + +2. `vtol_transition_mc_authority_end_percent` +- Defines MC stabilization authority scale at transition end. +- MC -> FW: MC authority goes from `100%` at start to `mc_authority_end_percent` at end. +- FW -> MC: MC authority goes from `mc_authority_end_percent` at start to `100%` at end. + +Example (`vtol_transition_mc_authority_end_percent = 30`): +- MC -> FW at 50% progress: MC authority is about 65%. +- FW -> MC at 50% progress: MC authority is about 65%. + +3. `vtol_transition_fw_authority_start_percent` +- Defines FW authority scale at transition start. +- MC -> FW: FW authority goes from `fw_authority_start_percent` at start to `100%` at end. +- FW -> MC: FW authority goes from `100%` at start to `fw_authority_start_percent` at end. + +Example (`vtol_transition_fw_authority_start_percent = 25`): +- MC -> FW at 50% progress: FW authority is about 62.5%. +- FW -> MC at 50% progress: FW authority is about 62.5%. + +Backward-compatible note: +- `vtol_transition_fw_authority_start_percent = 100` preserves legacy FW authority handoff behavior. +- Lower values provide smoother FW authority ramp-in/out. + +## CLI Commands (English) + +Use these commands in CLI (`set ...`, then `save`): + +- `set manual_vtol_transition_controller = ON|OFF` + - Enables edge-triggered manual transition controller. + +- `set vtol_transition_dynamic_mixer = ON|OFF` + - Enables/disables dynamic progress-based scaling. + +- `set vtol_transition_to_fw_min_airspeed_cm_s = ` + - Preferred MC -> FW completion threshold (pitot airspeed). + +- `set mixer_switch_trans_airspeed_cm_s = ` + - Legacy MC -> FW threshold, used when `vtol_transition_to_fw_min_airspeed_cm_s = 0`. + +- `set mixer_switch_trans_timer = ` + - Timer-based transition duration fallback (used when pitot airspeed is unavailable/unhealthy). + +- `set vtol_transition_to_mc_max_airspeed_cm_s = ` + - FW -> MC completion threshold (pitot airspeed). + +- `set vtol_transition_airspeed_timeout_ms = ` + - Transition timeout/abort window. + +- `set vtol_transition_lift_end_percent = <0..100>` + - Lift scale endpoint for dynamic transition. + +- `set vtol_transition_mc_authority_end_percent = <0..100>` + - MC authority endpoint for dynamic transition. + +- `set vtol_transition_fw_authority_start_percent = <0..100>` + - FW authority start level for dynamic transition. + +- `set nav_vtol_mission_transition_user_action = OFF|USER1|USER2|USER3|USER4` + - Selects waypoint User Action bit used for mission VTOL target selector. + +- `set nav_vtol_mission_transition_min_altitude_cm = ` + - Optional minimum altitude check before mission transition start (`0` disables). + +- `set nav_vtol_mission_transition_track_distance_cm = ` + - Straight-line transition guidance distance for mission MC -> FW segment. + # Notes and Experiences ## General diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 68b058e8d37..f9f0204e768 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1281,7 +1281,7 @@ groups: 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." + description: "Legacy MC->FW airspeed threshold [cm/s] for automated profile switch. Used when `vtol_transition_to_fw_min_airspeed_cm_s = 0`. If airspeed is unavailable, timer-based fallback (`mixer_switch_trans_timer`) is used." default_value: 0 field: mixer_config.switchTransitionAirspeed min: 0 @@ -1297,7 +1297,7 @@ groups: field: mixer_config.manualVtolTransitionController type: bool - name: vtol_transition_to_fw_min_airspeed_cm_s - description: "Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. If 0, `mixer_switch_trans_airspeed_cm_s` is used for MC->FW." + description: "Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. Overrides `mixer_switch_trans_airspeed_cm_s` when > 0. If 0, legacy setting is used." default_value: 0 field: mixer_config.vtolTransitionToFwMinAirspeed min: 0 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 030d56cc0ca..77038141281 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -649,7 +649,9 @@ void FAST_CODE mixTable(void) } //spin stopped motors only in mixer transition mode if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && !feature(FEATURE_REVERSIBLE_MOTORS)) { - motor[i] = -currentMixer[i].throttle * 1000 * pusherScale; + const float pusherTarget = -currentMixer[i].throttle * 1000.0f; + const float pusherIdle = throttleRangeMin; + motor[i] = pusherIdle + (pusherTarget - pusherIdle) * pusherScale; motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } } diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 208b0664d68..8cead491bae 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -131,7 +131,9 @@ void setMixerProfileAT(void) mixerProfileAT.aborted = false; mixerProfileAT.hotSwitchDone = false; mixerProfileAT.usedAirspeed = false; + mixerProfileAT.transitionStartAirspeedCaptured = false; mixerProfileAT.progress = 0.0f; + mixerProfileAT.transitionStartAirspeedCmS = 0.0f; mixerProfileAT.blendToFw = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW ? 0.0f : 1.0f; mixerProfileAT.pusherScale = 1.0f; mixerProfileAT.liftScale = 1.0f; @@ -171,6 +173,16 @@ static void resetTransitionScales(void) mixerProfileAT.fwAuthorityScale = 1.0f; } +static void setLegacyTransitionScales(void) +{ + mixerProfileAT.progress = 1.0f; + mixerProfileAT.blendToFw = 1.0f; + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; +} + static float blendScale(float from, float to, float progress) { return from + (to - from) * constrainf(progress, 0.0f, 1.0f); @@ -251,6 +263,8 @@ static void abortTransition(void) mixerProfileAT.hotSwitchDone = false; mixerProfileAT.request = MIXERAT_REQUEST_NONE; mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + mixerProfileAT.transitionStartAirspeedCaptured = false; + mixerProfileAT.transitionStartAirspeedCmS = 0.0f; resetTransitionScales(); } @@ -275,7 +289,19 @@ static bool mixerATReadyForHotSwitch(const mixerProfileATRequest_e required_acti return airspeedCmS >= airspeedThresholdCmS; } - mixerProfileAT.progress = constrainf((airspeedThresholdCmS - airspeedCmS) / airspeedThresholdCmS, 0.0f, 1.0f); + if (!mixerProfileAT.transitionStartAirspeedCaptured) { + mixerProfileAT.transitionStartAirspeedCmS = airspeedCmS; + mixerProfileAT.transitionStartAirspeedCaptured = true; + } + + const float startAirspeed = mixerProfileAT.transitionStartAirspeedCmS; + const float thresholdAirspeed = airspeedThresholdCmS; + if (startAirspeed <= thresholdAirspeed) { + mixerProfileAT.progress = 1.0f; + } else { + mixerProfileAT.progress = constrainf((startAirspeed - airspeedCmS) / (startAirspeed - thresholdAirspeed), 0.0f, 1.0f); + } + return airspeedCmS <= airspeedThresholdCmS; } @@ -454,6 +480,9 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) // Backward-compatible manual path: level-controlled transition mixing request. if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) { isMixerTransitionMixing_requested = transitionModeActive; + if (isMixerTransitionMixing_requested) { + setLegacyTransitionScales(); + } } manualTransitionReadyForEdge = true; } else { diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 1b3ae5f6f86..46fa19bf7d7 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -72,7 +72,9 @@ typedef struct mixerProfileAT_s { bool aborted; bool hotSwitchDone; bool usedAirspeed; + bool transitionStartAirspeedCaptured; float progress; + float transitionStartAirspeedCmS; float blendToFw; float pusherScale; float liftScale; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c78f7d8964d..1c0a09310c6 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2458,6 +2458,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState) { UNUSED(previousState); + + if (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE)) { + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + clearMissionVTOLTransitionState(); + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + mixerProfileATRequest_e required_action; switch (navMixerATPendingState) { From 232dfb5ca0de1ad738ec1b4211ebc53eca8a1535 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 12 May 2026 23:13:59 +0300 Subject: [PATCH 04/42] - document explicit USER-bit semantics as absolute per-waypoint platform target (0=MC, 1=FW) - document dependency on existing mixer profile switching infrastructure (two profiles + MIXER PROFILE 2 mode condition) - update docs: MixerProfile.md, Navigation.md, VTOL.md with behavior, safety boundaries, tuning examples, and CLI reference --- docs/MixerProfile.md | 8 ++++++++ docs/Navigation.md | 2 ++ docs/VTOL.md | 12 +++++++++++- src/main/fc/settings.yaml | 2 +- 4 files changed, 22 insertions(+), 2 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 64f9bc7a3c1..a05fcc1e36c 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -130,6 +130,7 @@ On each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`), the con - selected USER bit = `0` -> transition to MC / MULTIROTOR profile - selected USER bit = `1` -> transition to FW / AIRPLANE profile - When `nav_vtol_mission_transition_user_action != OFF`, each navigable waypoint encodes a target state via that selected bit. +- This is a per-waypoint target-state declaration (not an event trigger). Users should intentionally set/clear the selected USER bit on each navigable waypoint. - This is **not** a toggle command. - If already in the requested profile type, the action is treated as complete (idempotent). @@ -139,6 +140,13 @@ For MC -> FW mission transitions, navigation uses a straight acceleration segmen Mission path uses the same controller and completion logic as manual transition (airspeed-first, timer fallback). 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. +Mission VTOL transition still relies on normal profile-switch infrastructure: configure two mixer profiles and a valid `MIXER PROFILE 2` mode activation condition. + +Example smooth-start values (optional tuning baseline): +- `set vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_lift_end_percent = 30` +- `set vtol_transition_mc_authority_end_percent = 20` +- `set vtol_transition_fw_authority_start_percent = 20` ### Validation Matrix (PR / SITL / HITL) diff --git a/docs/Navigation.md b/docs/Navigation.md index 730ce10f7d4..9fb02f8a7a0 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -122,6 +122,7 @@ Behavior on each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`) - `0`: transition to MC / MULTIROTOR profile - `1`: transition to FW / AIRPLANE profile - When `nav_vtol_mission_transition_user_action != OFF`, each navigable waypoint always encodes target state via that selected USER bit. +- This means every navigable waypoint implicitly declares desired VTOL platform state when this feature is enabled; users must intentionally set/clear that bit on each waypoint. - 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. @@ -138,6 +139,7 @@ Transition behavior in this MVP: Safety and scope: - This path uses authorized automated transition state handling; it does not permit manual mixer profile switching during normal waypoint navigation. +- It still depends on valid mixer profile switching infrastructure (two configured mixer profiles and a valid `MIXER PROFILE 2` mode activation condition). `wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`). diff --git a/docs/VTOL.md b/docs/VTOL.md index 5efcd250951..c1f4ffa29c7 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -321,6 +321,7 @@ Mission transition is configured with `nav_vtol_mission_transition_user_action`. - `USER1`..`USER4`: selected User Action bit is used as target selector on navigable waypoints. - selected bit `0` -> target MC profile - selected bit `1` -> target FW profile +- when enabled, every navigable waypoint implicitly declares desired VTOL platform state through that selected bit, so users should set/clear it intentionally per waypoint - Mission progression pauses during transition and resumes only after completion. - If already in requested target profile, command is idempotent (no new transition). @@ -350,6 +351,12 @@ When `vtol_transition_dynamic_mixer = ON`, transition progress additionally scal When `vtol_transition_dynamic_mixer = OFF`, legacy static transition mixing behavior is preserved. +Optional smooth-start baseline: +- `vtol_transition_dynamic_mixer = ON` +- `vtol_transition_lift_end_percent = 30` +- `vtol_transition_mc_authority_end_percent = 20` +- `vtol_transition_fw_authority_start_percent = 20` + ### Detailed effect of the three percentage settings These three settings are active only when `vtol_transition_dynamic_mixer = ON`. @@ -420,7 +427,7 @@ Use these commands in CLI (`set ...`, then `save`): - FW authority start level for dynamic transition. - `set nav_vtol_mission_transition_user_action = OFF|USER1|USER2|USER3|USER4` - - Selects waypoint User Action bit used for mission VTOL target selector. + - Selects waypoint User Action bit used for mission VTOL target selector (absolute per-waypoint desired state). - `set nav_vtol_mission_transition_min_altitude_cm = ` - Optional minimum altitude check before mission transition start (`0` disables). @@ -428,6 +435,9 @@ Use these commands in CLI (`set ...`, then `save`): - `set nav_vtol_mission_transition_track_distance_cm = ` - Straight-line transition guidance distance for mission MC -> FW segment. +Mission profile-switch dependency: +- Mission VTOL transition uses the existing profile hot-switch path, so two valid mixer profiles and a configured `MIXER PROFILE 2` mode activation condition are required. + # Notes and Experiences ## General diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f9f0204e768..a5b8898f6e6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2679,7 +2679,7 @@ groups: 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." + 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. This is an absolute per-waypoint target-state selector and relies on existing mixer profile switching infrastructure (two profiles and valid MIXER PROFILE 2 mode activation condition)." default_value: "OFF" field: general.vtol_mission_transition_user_action table: nav_wp_user_action From 8ab311a1745337ab22933e4e7af5a899b39766d3 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Wed, 13 May 2026 15:32:37 +0300 Subject: [PATCH 05/42] feat(vtol): add optional dynamic scaling ramp timer and clarify legacy/3-pos switch behavior - add new mixer setting `vtol_transition_scale_ramp_time_ms` (default 0) - keep backward compatibility: - `0` => scaling stays coupled to transition progress (existing behavior) - `>0` => pusher/lift/authority scaling uses time-based ramp - keep transition completion logic unchanged: - airspeed-first when pitot is healthy/available - timer fallback via `mixer_switch_trans_timer` when pitot is unavailable/unhealthy - wire new setting into mixer profile config/reset path - update VTOL and MixerProfile docs: - explicitly state intent is not to replace legacy manual behavior - document 3-position workflow with edge-trigger controller - document new ramp timer semantics with practical examples --- docs/MixerProfile.md | 19 +++++++++- docs/VTOL.md | 67 +++++++++++++++++++++++++++++++++ src/main/fc/settings.yaml | 6 +++ src/main/flight/mixer_profile.c | 32 ++++++++++++---- src/main/flight/mixer_profile.h | 1 + 5 files changed, 116 insertions(+), 9 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index a05fcc1e36c..a62cc909f3f 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -87,7 +87,9 @@ When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Mod Manual `MIXER TRANSITION` and mission-authorized VTOL transition both use the same internal transition controller. This controller always computes transition progress/completion and performs profile hot-switch only inside the authorized transition state. -When `vtol_transition_dynamic_mixer = ON`, that progress is also used for pusher/lift/authority scaling. +When `vtol_transition_dynamic_mixer = ON`, pusher/lift/authority scaling is enabled and is driven by: +- transition progress (default), or +- `vtol_transition_scale_ramp_time_ms` when configured (>0). ### Airspeed-first completion @@ -116,6 +118,21 @@ When `vtol_transition_dynamic_mixer = ON`, transition progress scales: Default is OFF to preserve existing behavior. With dynamic scaling enabled, `vtol_transition_fw_authority_start_percent = 100` preserves legacy FW authority handoff; lower values provide smoother ramp-in. +Optional scaling ramp timer: + +- `vtol_transition_scale_ramp_time_ms = 0` (default): scaling remains coupled to transition progress (legacy-compatible behavior). +- `vtol_transition_scale_ramp_time_ms > 0`: scaling uses this timer, while transition completion stays airspeed-first (or timer fallback if pitot unavailable/unhealthy). + +Example: + +- `mixer_switch_trans_timer = 50` (5s fallback completion timer) +- `vtol_transition_scale_ramp_time_ms = 1200` + +Result: +- scaling reaches target levels in ~1.2s, +- transition completion still follows airspeed threshold when pitot is healthy, +- timer fallback completion still uses 5s when pitot is unavailable/unhealthy. + ### Mission-authorized VTOL transition (waypoint User Action) INAV supports mission-requested VTOL transitions through the existing automated transition path. This is configured with: diff --git a/docs/VTOL.md b/docs/VTOL.md index c1f4ffa29c7..0807ffe4ef6 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -302,6 +302,8 @@ This keeps one safety boundary for profile hot-switching and avoids separate tra ### Manual transition semantics +Intent: this does not replace legacy manual behavior. Legacy remains available and selectable. + With `manual_vtol_transition_controller = ON`: - `MIXER TRANSITION` acts as an edge-triggered request. - A rising edge starts one transition. @@ -313,6 +315,15 @@ With `manual_vtol_transition_controller = ON`: With `manual_vtol_transition_controller = OFF`: - legacy manual behavior is preserved for backward compatibility. +Typical 3-position switch workflow (edge-trigger mode enabled): +- Position 1: MC +- Position 2: Transition (trigger AUTO transition sequence) +- Position 3: FW + +Operational example: +- fly in MC (pos1) -> move to Transition (pos2) to start automatic MC->FW transition -> after completion move to FW (pos3), +- reverse order for FW->MC. + ### Mission-authorized transition semantics Mission transition is configured with `nav_vtol_mission_transition_user_action`. @@ -351,6 +362,19 @@ When `vtol_transition_dynamic_mixer = ON`, transition progress additionally scal When `vtol_transition_dynamic_mixer = OFF`, legacy static transition mixing behavior is preserved. +Optional decoupled scaling ramp: +- `vtol_transition_scale_ramp_time_ms = 0` (default): scaling follows transition progress (legacy-compatible behavior). +- `vtol_transition_scale_ramp_time_ms > 0`: scaling uses this ramp timer, while completion logic remains unchanged (airspeed-first; timer fallback when pitot is unavailable/unhealthy). + +Example: +- `mixer_switch_trans_timer = 50` (5s fallback completion timer) +- `vtol_transition_scale_ramp_time_ms = 1200` + +Result: +- pusher/lift/authority scaling reaches target levels in ~1.2s, +- transition completion still follows airspeed thresholds when pitot is healthy, +- if pitot is unavailable/unhealthy, completion fallback still uses 5s. + Optional smooth-start baseline: - `vtol_transition_dynamic_mixer = ON` - `vtol_transition_lift_end_percent = 30` @@ -417,6 +441,9 @@ Use these commands in CLI (`set ...`, then `save`): - `set vtol_transition_airspeed_timeout_ms = ` - Transition timeout/abort window. +- `set vtol_transition_scale_ramp_time_ms = ` + - Optional dynamic scaling ramp duration in milliseconds. `0` keeps legacy progress-coupled scaling. `>0` decouples scaling ramp time from completion timing. + - `set vtol_transition_lift_end_percent = <0..100>` - Lift scale endpoint for dynamic transition. @@ -452,3 +479,43 @@ Mission profile-switch dependency: - There will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. Use the max speed or faster speed in tiling servo to reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect. ## Dedicated forward motor - Easiest way to setup a vtol. and efficiency can be improved by using different motor/prop for hover and forward flight + +## Pitot-based transition logic (reference) + +When pitot is healthy/available, transition progress is airspeed-driven (not timer-driven). + +- MC -> FW: + - progress = `constrain(airspeed / to_fw_threshold, 0..1)` + - completion condition = `airspeed >= to_fw_threshold` + +- FW -> MC: + - capture `startAirspeed` when transition starts + - progress = `constrain((startAirspeed - airspeed) / (startAirspeed - to_mc_threshold), 0..1)` + - completion condition = `airspeed <= to_mc_threshold` + +Dynamic mixer scaling (`vtol_transition_dynamic_mixer = ON`) uses this progress: + +- MC -> FW: + - pusher scale ramps `0 -> 1` + - lift scale ramps `1 -> vtol_transition_lift_end_percent` + - MC authority ramps `1 -> vtol_transition_mc_authority_end_percent` + - FW authority ramps `vtol_transition_fw_authority_start_percent -> 1` + +- FW -> MC: + - pusher scale ramps `1 -> 0` + - lift scale ramps `vtol_transition_lift_end_percent -> 1` + - MC authority ramps `vtol_transition_mc_authority_end_percent -> 1` + - FW authority ramps `1 -> vtol_transition_fw_authority_start_percent` + +If `vtol_transition_scale_ramp_time_ms > 0`, dynamic scaling uses that timer-based ramp instead of transition-progress coupling. +This changes only scaling shape. Transition completion logic remains airspeed-first (with timer fallback when pitot is unavailable/unhealthy). + +For transition/pusher motors (`-2.0 < throttle < -1.0`), output is interpolated from idle to target: + +`motor = idle + (target - idle) * pusherScale` + +where: +- `target = -mixerThrottle * 1000` +- `idle = throttleRangeMin` + +If pitot is unavailable/unhealthy, timer fallback is used (`mixer_switch_trans_timer`). diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a5b8898f6e6..9ea3b64966e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1314,6 +1314,12 @@ groups: field: mixer_config.vtolTransitionAirspeedTimeoutMs min: 0 max: 60000 + - name: vtol_transition_scale_ramp_time_ms + description: "Optional dynamic scaling ramp duration [ms]. When > 0 and `vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling uses this timer instead of transition completion progress. Set to 0 to keep legacy progress-coupled scaling behavior." + default_value: 0 + field: mixer_config.vtolTransitionScaleRampTimeMs + min: 0 + max: 60000 - name: vtol_transition_lift_end_percent description: "Target vertical-lift throttle scale at transition end, in percent. Used only when `vtol_transition_dynamic_mixer` is ON." default_value: 100 diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 8cead491bae..47d5632762c 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -64,6 +64,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .vtolTransitionToFwMinAirspeed = SETTING_VTOL_TRANSITION_TO_FW_MIN_AIRSPEED_CM_S_DEFAULT, .vtolTransitionToMcMaxAirspeed = SETTING_VTOL_TRANSITION_TO_MC_MAX_AIRSPEED_CM_S_DEFAULT, .vtolTransitionAirspeedTimeoutMs = SETTING_VTOL_TRANSITION_AIRSPEED_TIMEOUT_MS_DEFAULT, + .vtolTransitionScaleRampTimeMs = SETTING_VTOL_TRANSITION_SCALE_RAMP_TIME_MS_DEFAULT, .vtolTransitionLiftEndPercent = SETTING_VTOL_TRANSITION_LIFT_END_PERCENT_DEFAULT, .vtolTransitionMcAuthorityEndPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_END_PERCENT_DEFAULT, .vtolTransitionFwAuthorityStartPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_START_PERCENT_DEFAULT, @@ -188,6 +189,20 @@ static float blendScale(float from, float to, float progress) return from + (to - from) * constrainf(progress, 0.0f, 1.0f); } +static float getScalingProgress(void) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer) { + return 1.0f; + } + + if (currentMixerConfig.vtolTransitionScaleRampTimeMs > 0) { + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + return constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); + } + + return constrainf(mixerProfileAT.progress, 0.0f, 1.0f); +} + static bool hasTrustedPitotAirspeed(float *airspeedCmS) { #ifdef USE_PITOT @@ -238,17 +253,18 @@ static void updateTransitionScales(void) const float liftFloor = constrainf(currentMixerConfig.vtolTransitionLiftEndPercent / 100.0f, 0.0f, 1.0f); const float mcFloor = constrainf(currentMixerConfig.vtolTransitionMcAuthorityEndPercent / 100.0f, 0.0f, 1.0f); const float fwFloor = constrainf(currentMixerConfig.vtolTransitionFwAuthorityStartPercent / 100.0f, 0.0f, 1.0f); + const float scaleProgress = getScalingProgress(); if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { - mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, mixerProfileAT.progress); - mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, mixerProfileAT.progress); - mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, mixerProfileAT.progress); - mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, mixerProfileAT.progress); + mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, scaleProgress); + mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, scaleProgress); + mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, scaleProgress); + mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, scaleProgress); } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { - mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, mixerProfileAT.progress); - mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, mixerProfileAT.progress); - mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, mixerProfileAT.progress); - mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, mixerProfileAT.progress); + mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, scaleProgress); + mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, scaleProgress); + mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, scaleProgress); + mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, scaleProgress); } mixerProfileAT.blendToFw = constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 46fa19bf7d7..784463eba7d 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -24,6 +24,7 @@ typedef struct mixerConfig_s { uint16_t vtolTransitionToFwMinAirspeed; uint16_t vtolTransitionToMcMaxAirspeed; uint16_t vtolTransitionAirspeedTimeoutMs; + uint16_t vtolTransitionScaleRampTimeMs; uint8_t vtolTransitionLiftEndPercent; uint8_t vtolTransitionMcAuthorityEndPercent; uint8_t vtolTransitionFwAuthorityStartPercent; From 50612a2a2570a5d1bfb7f4b267a2e95491e828cd Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 14 May 2026 16:17:39 +0300 Subject: [PATCH 06/42] docs: regenerate Settings.md from settings definitions --- docs/Settings.md | 134 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 134 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index d49067357bc..7b0e1073d61 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2824,6 +2824,16 @@ Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% --- +### manual_vtol_transition_controller + +Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### manual_yaw_rate Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% @@ -3199,6 +3209,16 @@ If enabled, control_profile_index will follow mixer_profile index. Set to OFF(de --- +### mixer_switch_trans_airspeed_cm_s + +Legacy MC->FW airspeed threshold [cm/s] for automated profile switch. Used when `vtol_transition_to_fw_min_airspeed_cm_s = 0`. If airspeed is unavailable, timer-based fallback (`mixer_switch_trans_timer`) is used. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 10000 | + +--- + ### mixer_switch_trans_timer If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. @@ -4626,6 +4646,40 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT --- +### nav_vtol_mission_transition_min_altitude_cm + +Minimum altitude [cm] required to start a mission-authorized VTOL transition. Set to 0 to disable the minimum-altitude check. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 50000 | + +--- + +### nav_vtol_mission_transition_track_distance_cm + +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 | Min | Max | +| --- | --- | --- | +| 100000 | 1000 | 500000 | + +--- + +### nav_vtol_mission_transition_user_action + +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. This is an absolute per-waypoint target-state selector and relies on existing mixer profile switching infrastructure (two profiles and valid MIXER PROFILE 2 mode activation condition). + +| Allowed Values | | +| --- | --- | +| OFF | Default | +| USER1 | | +| USER2 | | +| USER3 | | +| USER4 | | + +--- + ### nav_wp_enforce_altitude Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on. 0 = disabled, otherwise setting defines altitude capture tolerance [cm], e.g. 100 means required altitude is achieved when within 100cm of waypoint altitude setting. @@ -6974,6 +7028,86 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, --- +### vtol_transition_airspeed_timeout_ms + +Safety timeout [ms] for airspeed-controlled transitions. If non-zero and required airspeed condition is not met in time, transition aborts instead of force-completing. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + +### vtol_transition_dynamic_mixer + +Enables dynamic VTOL transition progress/scaling controller shared by mission-authorized and manual MIXER TRANSITION paths. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### vtol_transition_fw_authority_start_percent + +Initial fixed-wing authority scale at transition start, in percent. Used only when `vtol_transition_dynamic_mixer` is ON. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_lift_end_percent + +Target vertical-lift throttle scale at transition end, in percent. Used only when `vtol_transition_dynamic_mixer` is ON. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_mc_authority_end_percent + +Target multicopter stabilization authority scale at transition end, in percent. Used only when `vtol_transition_dynamic_mixer` is ON. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_scale_ramp_time_ms + +Optional dynamic scaling ramp duration [ms]. When > 0 and `vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling uses this timer instead of transition completion progress. Set to 0 to keep legacy progress-coupled scaling behavior. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + +### vtol_transition_to_fw_min_airspeed_cm_s + +Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. Overrides `mixer_switch_trans_airspeed_cm_s` when > 0. If 0, legacy setting is used. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20000 | + +--- + +### vtol_transition_to_mc_max_airspeed_cm_s + +Maximum pitot airspeed [cm/s] allowed to complete FW->MC transition when airspeed is healthy and available. If 0, FW->MC uses timer fallback. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20000 | + +--- + ### vtx_band Configure the VTX band. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. From 8d781e9e0278dadca216ab4571e9c19f657d8106 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Sat, 16 May 2026 16:59:47 +0300 Subject: [PATCH 07/42] vtol: split global vs per-mixer transition settings, rename manual switch controller setting - Rename per-mixer manual transition setting: - `mixer_manual_vtol_transition_controller` -> `mixer_vtol_manualswitch_autotransition_controller` - Keep per-mixer scope only where profile-specific behavior is intended: - `mixer_vtol_transition_dynamic_mixer` - `mixer_vtol_transition_airspeed_timeout_ms` - `mixer_vtol_transition_scale_ramp_time_ms` - legacy/profile-switch settings (`mixer_automated_switch`, `mixer_switch_trans_*`) - Move these transition tuning parameters from mixer profile scope to global system scope: - `vtol_transition_to_fw_min_airspeed_cm_s` - `vtol_transition_to_mc_max_airspeed_cm_s` - `vtol_transition_lift_end_percent` - `vtol_transition_mc_authority_end_percent` - `vtol_transition_fw_authority_start_percent` - Update transition logic to read moved fields from `systemConfig()` instead of `currentMixerConfig` - Remove moved fields from `mixerConfig_t`; add them to `systemConfig_t` - Bump PG versions for layout changes: - `PG_MIXER_PROFILE`: 2 -> 3 - `PG_SYSTEM_CONFIG`: 7 -> 8 - Update docs and regenerate CLI settings docs: - explicit per-mixer vs global scope notes in VTOL/MixerProfile docs - `docs/Settings.md` regenerated via `update_cli_docs.py` --- docs/MixerProfile.md | 40 +++++++++++---- docs/Settings.md | 86 ++++++++++++++++----------------- docs/VTOL.md | 61 +++++++++++++++++------ src/main/fc/config.c | 7 ++- src/main/fc/config.h | 5 ++ src/main/fc/settings.yaml | 70 +++++++++++++-------------- src/main/flight/mixer_profile.c | 27 +++++------ src/main/flight/mixer_profile.h | 5 -- 8 files changed, 175 insertions(+), 126 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index a62cc909f3f..60f103eb6e4 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -30,8 +30,8 @@ The use of Transition Mode is recommended to enable further features and future - A new transition requires mode OFF then ON again. - If switched OFF before hot-switch completes, the manual transition request is aborted. -This edge-triggered behavior is enabled by `manual_vtol_transition_controller`. -When `manual_vtol_transition_controller = OFF`, manual transition keeps legacy behavior. +This edge-triggered behavior is enabled by `mixer_vtol_manualswitch_autotransition_controller`. +When `mixer_vtol_manualswitch_autotransition_controller = OFF`, manual transition keeps legacy behavior. ## Servo @@ -87,9 +87,9 @@ When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Mod Manual `MIXER TRANSITION` and mission-authorized VTOL transition both use the same internal transition controller. This controller always computes transition progress/completion and performs profile hot-switch only inside the authorized transition state. -When `vtol_transition_dynamic_mixer = ON`, pusher/lift/authority scaling is enabled and is driven by: +When `mixer_vtol_transition_dynamic_mixer = ON`, pusher/lift/authority scaling is enabled and is driven by: - transition progress (default), or -- `vtol_transition_scale_ramp_time_ms` when configured (>0). +- `mixer_vtol_transition_scale_ramp_time_ms` when configured (>0). ### Airspeed-first completion @@ -104,11 +104,11 @@ Ground speed is not used for transition completion/progress. Optional safety timeout: -- `vtol_transition_airspeed_timeout_ms` can abort transition if airspeed condition is not met in time. +- `mixer_vtol_transition_airspeed_timeout_ms` can abort transition if airspeed condition is not met in time. ### Dynamic scaling (optional) -When `vtol_transition_dynamic_mixer = ON`, transition progress scales: +When `mixer_vtol_transition_dynamic_mixer = ON`, transition progress scales: - pusher contribution (`-2.0 < throttle < -1.0` motors) from configured max toward 0/100% depending on direction, - lift motor throttle contribution (`vtol_transition_lift_end_percent`), @@ -120,13 +120,13 @@ With dynamic scaling enabled, `vtol_transition_fw_authority_start_percent = 100` Optional scaling ramp timer: -- `vtol_transition_scale_ramp_time_ms = 0` (default): scaling remains coupled to transition progress (legacy-compatible behavior). -- `vtol_transition_scale_ramp_time_ms > 0`: scaling uses this timer, while transition completion stays airspeed-first (or timer fallback if pitot unavailable/unhealthy). +- `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): scaling remains coupled to transition progress (legacy-compatible behavior). +- `mixer_vtol_transition_scale_ramp_time_ms > 0`: scaling uses this timer, while transition completion stays airspeed-first (or timer fallback if pitot unavailable/unhealthy). Example: - `mixer_switch_trans_timer = 50` (5s fallback completion timer) -- `vtol_transition_scale_ramp_time_ms = 1200` +- `mixer_vtol_transition_scale_ramp_time_ms = 1200` Result: - scaling reaches target levels in ~1.2s, @@ -142,6 +142,26 @@ INAV supports mission-requested VTOL transitions through the existing automated - `vtol_transition_to_fw_min_airspeed_cm_s` (preferred MC->FW threshold) - `mixer_switch_trans_airspeed_cm_s` (legacy MC->FW fallback when preferred threshold is `0`) +Scope note: + +- Per-mixer-profile settings: + - `mixer_automated_switch` + - `mixer_switch_trans_timer` + - `mixer_switch_trans_airspeed_cm_s` + - `mixer_vtol_transition_dynamic_mixer` + - `mixer_vtol_manualswitch_autotransition_controller` + - `mixer_vtol_transition_airspeed_timeout_ms` + - `mixer_vtol_transition_scale_ramp_time_ms` +- Global settings: + - `vtol_transition_to_fw_min_airspeed_cm_s` + - `vtol_transition_to_mc_max_airspeed_cm_s` + - `vtol_transition_lift_end_percent` + - `vtol_transition_mc_authority_end_percent` + - `vtol_transition_fw_authority_start_percent` + - `nav_vtol_mission_transition_user_action` + - `nav_vtol_mission_transition_min_altitude_cm` + - `nav_vtol_mission_transition_track_distance_cm` + 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 @@ -160,7 +180,7 @@ Manual RC switching (`MIXER PROFILE 2`, `MIXER TRANSITION`) remains blocked duri Mission VTOL transition still relies on normal profile-switch infrastructure: configure two mixer profiles and a valid `MIXER PROFILE 2` mode activation condition. Example smooth-start values (optional tuning baseline): -- `set vtol_transition_dynamic_mixer = ON` +- `set mixer_vtol_transition_dynamic_mixer = ON` - `set vtol_transition_lift_end_percent = 30` - `set vtol_transition_mc_authority_end_percent = 20` - `set vtol_transition_fw_authority_start_percent = 20` diff --git a/docs/Settings.md b/docs/Settings.md index 7b0e1073d61..7d7c0e06f18 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2824,16 +2824,6 @@ Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% --- -### manual_vtol_transition_controller - -Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior. - -| Default | Min | Max | -| --- | --- | --- | -| OFF | OFF | ON | - ---- - ### manual_yaw_rate Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% @@ -3229,6 +3219,46 @@ If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_ --- +### mixer_vtol_manualswitch_autotransition_controller + +Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mixer_vtol_transition_airspeed_timeout_ms + +Safety timeout [ms] for airspeed-controlled transitions. If non-zero and required airspeed condition is not met in time, transition aborts instead of force-completing. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + +### mixer_vtol_transition_dynamic_mixer + +Enables dynamic VTOL transition progress/scaling controller shared by mission-authorized and manual MIXER TRANSITION paths. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mixer_vtol_transition_scale_ramp_time_ms + +Optional dynamic scaling ramp duration [ms]. When > 0 and `mixer_vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling uses this timer instead of transition completion progress. Set to 0 to keep legacy progress-coupled scaling behavior. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + ### mode_range_logic_operator Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. @@ -7028,29 +7058,9 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, --- -### vtol_transition_airspeed_timeout_ms - -Safety timeout [ms] for airspeed-controlled transitions. If non-zero and required airspeed condition is not met in time, transition aborts instead of force-completing. - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | 60000 | - ---- - -### vtol_transition_dynamic_mixer - -Enables dynamic VTOL transition progress/scaling controller shared by mission-authorized and manual MIXER TRANSITION paths. - -| Default | Min | Max | -| --- | --- | --- | -| OFF | OFF | ON | - ---- - ### vtol_transition_fw_authority_start_percent -Initial fixed-wing authority scale at transition start, in percent. Used only when `vtol_transition_dynamic_mixer` is ON. +Initial fixed-wing authority scale at transition start, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. | Default | Min | Max | | --- | --- | --- | @@ -7060,7 +7070,7 @@ Initial fixed-wing authority scale at transition start, in percent. Used only wh ### vtol_transition_lift_end_percent -Target vertical-lift throttle scale at transition end, in percent. Used only when `vtol_transition_dynamic_mixer` is ON. +Target vertical-lift throttle scale at transition end, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. | Default | Min | Max | | --- | --- | --- | @@ -7070,7 +7080,7 @@ Target vertical-lift throttle scale at transition end, in percent. Used only whe ### vtol_transition_mc_authority_end_percent -Target multicopter stabilization authority scale at transition end, in percent. Used only when `vtol_transition_dynamic_mixer` is ON. +Target multicopter stabilization authority scale at transition end, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. | Default | Min | Max | | --- | --- | --- | @@ -7078,16 +7088,6 @@ Target multicopter stabilization authority scale at transition end, in percent. --- -### vtol_transition_scale_ramp_time_ms - -Optional dynamic scaling ramp duration [ms]. When > 0 and `vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling uses this timer instead of transition completion progress. Set to 0 to keep legacy progress-coupled scaling behavior. - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | 60000 | - ---- - ### vtol_transition_to_fw_min_airspeed_cm_s Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. Overrides `mixer_switch_trans_airspeed_cm_s` when > 0. If 0, legacy setting is used. diff --git a/docs/VTOL.md b/docs/VTOL.md index 0807ffe4ef6..d3e4c96ab40 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -304,7 +304,7 @@ This keeps one safety boundary for profile hot-switching and avoids separate tra Intent: this does not replace legacy manual behavior. Legacy remains available and selectable. -With `manual_vtol_transition_controller = ON`: +With `mixer_vtol_manualswitch_autotransition_controller = ON`: - `MIXER TRANSITION` acts as an edge-triggered request. - A rising edge starts one transition. - Transition then runs autonomously to completion. @@ -312,7 +312,7 @@ With `manual_vtol_transition_controller = ON`: - To start another transition, mode must go OFF then ON again. - If mode is turned OFF before hot-switch, transition request is aborted safely. -With `manual_vtol_transition_controller = OFF`: +With `mixer_vtol_manualswitch_autotransition_controller = OFF`: - legacy manual behavior is preserved for backward compatibility. Typical 3-position switch workflow (edge-trigger mode enabled): @@ -350,25 +350,25 @@ FW -> MC: - completion threshold: `vtol_transition_to_mc_max_airspeed_cm_s` Timeout: -- `vtol_transition_airspeed_timeout_ms` can abort transition if condition is not achieved in time. +- `mixer_vtol_transition_airspeed_timeout_ms` can abort transition if condition is not achieved in time. ### Dynamic mixer scaling -When `vtol_transition_dynamic_mixer = ON`, transition progress additionally scales: +When `mixer_vtol_transition_dynamic_mixer = ON`, transition progress additionally scales: - pusher transition contribution, - vertical lift contribution, - MC stabilization authority, - FW transition input authority blend. -When `vtol_transition_dynamic_mixer = OFF`, legacy static transition mixing behavior is preserved. +When `mixer_vtol_transition_dynamic_mixer = OFF`, legacy static transition mixing behavior is preserved. Optional decoupled scaling ramp: -- `vtol_transition_scale_ramp_time_ms = 0` (default): scaling follows transition progress (legacy-compatible behavior). -- `vtol_transition_scale_ramp_time_ms > 0`: scaling uses this ramp timer, while completion logic remains unchanged (airspeed-first; timer fallback when pitot is unavailable/unhealthy). +- `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): scaling follows transition progress (legacy-compatible behavior). +- `mixer_vtol_transition_scale_ramp_time_ms > 0`: scaling uses this ramp timer, while completion logic remains unchanged (airspeed-first; timer fallback when pitot is unavailable/unhealthy). Example: - `mixer_switch_trans_timer = 50` (5s fallback completion timer) -- `vtol_transition_scale_ramp_time_ms = 1200` +- `mixer_vtol_transition_scale_ramp_time_ms = 1200` Result: - pusher/lift/authority scaling reaches target levels in ~1.2s, @@ -376,14 +376,14 @@ Result: - if pitot is unavailable/unhealthy, completion fallback still uses 5s. Optional smooth-start baseline: -- `vtol_transition_dynamic_mixer = ON` +- `mixer_vtol_transition_dynamic_mixer = ON` - `vtol_transition_lift_end_percent = 30` - `vtol_transition_mc_authority_end_percent = 20` - `vtol_transition_fw_authority_start_percent = 20` ### Detailed effect of the three percentage settings -These three settings are active only when `vtol_transition_dynamic_mixer = ON`. +These three settings are active only when `mixer_vtol_transition_dynamic_mixer = ON`. 1. `vtol_transition_lift_end_percent` - Defines lift throttle scale at transition end. @@ -416,14 +416,43 @@ Backward-compatible note: - `vtol_transition_fw_authority_start_percent = 100` preserves legacy FW authority handoff behavior. - Lower values provide smoother FW authority ramp-in/out. +## Setting Scope (Important) + +The new VTOL settings are split into two groups: + +### Per-mixer-profile settings + +These can differ between mixer profile 1 (typically MC) and mixer profile 2 (typically FW): + +- `mixer_automated_switch` +- `mixer_switch_trans_timer` +- `mixer_switch_trans_airspeed_cm_s` +- `mixer_vtol_transition_dynamic_mixer` +- `mixer_vtol_manualswitch_autotransition_controller` +- `mixer_vtol_transition_airspeed_timeout_ms` +- `mixer_vtol_transition_scale_ramp_time_ms` + +### Global settings + +These are shared system-wide and are not profile-specific: + +- `vtol_transition_to_fw_min_airspeed_cm_s` +- `vtol_transition_to_mc_max_airspeed_cm_s` +- `vtol_transition_lift_end_percent` +- `vtol_transition_mc_authority_end_percent` +- `vtol_transition_fw_authority_start_percent` +- `nav_vtol_mission_transition_user_action` +- `nav_vtol_mission_transition_min_altitude_cm` +- `nav_vtol_mission_transition_track_distance_cm` + ## CLI Commands (English) Use these commands in CLI (`set ...`, then `save`): -- `set manual_vtol_transition_controller = ON|OFF` +- `set mixer_vtol_manualswitch_autotransition_controller = ON|OFF` - Enables edge-triggered manual transition controller. -- `set vtol_transition_dynamic_mixer = ON|OFF` +- `set mixer_vtol_transition_dynamic_mixer = ON|OFF` - Enables/disables dynamic progress-based scaling. - `set vtol_transition_to_fw_min_airspeed_cm_s = ` @@ -438,10 +467,10 @@ Use these commands in CLI (`set ...`, then `save`): - `set vtol_transition_to_mc_max_airspeed_cm_s = ` - FW -> MC completion threshold (pitot airspeed). -- `set vtol_transition_airspeed_timeout_ms = ` +- `set mixer_vtol_transition_airspeed_timeout_ms = ` - Transition timeout/abort window. -- `set vtol_transition_scale_ramp_time_ms = ` +- `set mixer_vtol_transition_scale_ramp_time_ms = ` - Optional dynamic scaling ramp duration in milliseconds. `0` keeps legacy progress-coupled scaling. `>0` decouples scaling ramp time from completion timing. - `set vtol_transition_lift_end_percent = <0..100>` @@ -493,7 +522,7 @@ When pitot is healthy/available, transition progress is airspeed-driven (not tim - progress = `constrain((startAirspeed - airspeed) / (startAirspeed - to_mc_threshold), 0..1)` - completion condition = `airspeed <= to_mc_threshold` -Dynamic mixer scaling (`vtol_transition_dynamic_mixer = ON`) uses this progress: +Dynamic mixer scaling (`mixer_vtol_transition_dynamic_mixer = ON`) uses this progress: - MC -> FW: - pusher scale ramps `0 -> 1` @@ -507,7 +536,7 @@ Dynamic mixer scaling (`vtol_transition_dynamic_mixer = ON`) uses this progress: - MC authority ramps `vtol_transition_mc_authority_end_percent -> 1` - FW authority ramps `1 -> vtol_transition_fw_authority_start_percent` -If `vtol_transition_scale_ramp_time_ms > 0`, dynamic scaling uses that timer-based ramp instead of transition-progress coupling. +If `mixer_vtol_transition_scale_ramp_time_ms > 0`, dynamic scaling uses that timer-based ramp instead of transition-progress coupling. This changes only scaling shape. Transition completion logic remains airspeed-first (with timer fallback when pitot is unavailable/unhealthy). For transition/pusher motors (`-2.0 < throttle < -1.0`), output is interpolated from idle to target: diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d3021317ae5..23d62fa40ac 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -103,7 +103,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 8); PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, @@ -117,6 +117,11 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .i2c_speed = SETTING_I2C_SPEED_DEFAULT, #endif .throttle_tilt_compensation_strength = SETTING_THROTTLE_TILT_COMP_STR_DEFAULT, // 0-100, 0 - disabled + .vtolTransitionToFwMinAirspeed = SETTING_VTOL_TRANSITION_TO_FW_MIN_AIRSPEED_CM_S_DEFAULT, + .vtolTransitionToMcMaxAirspeed = SETTING_VTOL_TRANSITION_TO_MC_MAX_AIRSPEED_CM_S_DEFAULT, + .vtolTransitionLiftEndPercent = SETTING_VTOL_TRANSITION_LIFT_END_PERCENT_DEFAULT, + .vtolTransitionMcAuthorityEndPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_END_PERCENT_DEFAULT, + .vtolTransitionFwAuthorityStartPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_START_PERCENT_DEFAULT, .craftName = SETTING_NAME_DEFAULT, .pilotName = SETTING_NAME_DEFAULT ); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index e3bde5f3eb7..c7f2501fceb 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -78,6 +78,11 @@ typedef struct systemConfig_s { uint8_t i2c_speed; #endif uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. + uint16_t vtolTransitionToFwMinAirspeed; + uint16_t vtolTransitionToMcMaxAirspeed; + uint8_t vtolTransitionLiftEndPercent; + uint8_t vtolTransitionMcAuthorityEndPercent; + uint8_t vtolTransitionFwAuthorityStartPercent; char craftName[MAX_NAME_LENGTH + 1]; char pilotName[MAX_NAME_LENGTH + 1]; } systemConfig_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9ea3b64966e..24c64b88f53 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1286,58 +1286,28 @@ groups: field: mixer_config.switchTransitionAirspeed min: 0 max: 10000 - - name: vtol_transition_dynamic_mixer + - name: mixer_vtol_transition_dynamic_mixer description: "Enables dynamic VTOL transition progress/scaling controller shared by mission-authorized and manual MIXER TRANSITION paths." default_value: OFF field: mixer_config.vtolTransitionDynamicMixer type: bool - - name: manual_vtol_transition_controller + - name: mixer_vtol_manualswitch_autotransition_controller description: "Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior." default_value: OFF field: mixer_config.manualVtolTransitionController type: bool - - name: vtol_transition_to_fw_min_airspeed_cm_s - description: "Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. Overrides `mixer_switch_trans_airspeed_cm_s` when > 0. If 0, legacy setting is used." - default_value: 0 - field: mixer_config.vtolTransitionToFwMinAirspeed - min: 0 - max: 20000 - - name: vtol_transition_to_mc_max_airspeed_cm_s - description: "Maximum pitot airspeed [cm/s] allowed to complete FW->MC transition when airspeed is healthy and available. If 0, FW->MC uses timer fallback." - default_value: 0 - field: mixer_config.vtolTransitionToMcMaxAirspeed - min: 0 - max: 20000 - - name: vtol_transition_airspeed_timeout_ms + - name: mixer_vtol_transition_airspeed_timeout_ms description: "Safety timeout [ms] for airspeed-controlled transitions. If non-zero and required airspeed condition is not met in time, transition aborts instead of force-completing." default_value: 0 field: mixer_config.vtolTransitionAirspeedTimeoutMs min: 0 max: 60000 - - name: vtol_transition_scale_ramp_time_ms - description: "Optional dynamic scaling ramp duration [ms]. When > 0 and `vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling uses this timer instead of transition completion progress. Set to 0 to keep legacy progress-coupled scaling behavior." + - name: mixer_vtol_transition_scale_ramp_time_ms + description: "Optional dynamic scaling ramp duration [ms]. When > 0 and `mixer_vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling uses this timer instead of transition completion progress. Set to 0 to keep legacy progress-coupled scaling behavior." default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs min: 0 max: 60000 - - name: vtol_transition_lift_end_percent - description: "Target vertical-lift throttle scale at transition end, in percent. Used only when `vtol_transition_dynamic_mixer` is ON." - default_value: 100 - field: mixer_config.vtolTransitionLiftEndPercent - min: 0 - max: 100 - - name: vtol_transition_mc_authority_end_percent - description: "Target multicopter stabilization authority scale at transition end, in percent. Used only when `vtol_transition_dynamic_mixer` is ON." - default_value: 100 - field: mixer_config.vtolTransitionMcAuthorityEndPercent - min: 0 - max: 100 - - name: vtol_transition_fw_authority_start_percent - description: "Initial fixed-wing authority scale at transition start, in percent. Used only when `vtol_transition_dynamic_mixer` is ON." - default_value: 100 - field: mixer_config.vtolTransitionFwAuthorityStartPercent - min: 0 - max: 100 - name: tailsitter_orientation_offset description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode" default_value: OFF @@ -4035,6 +4005,36 @@ groups: field: throttle_tilt_compensation_strength min: 0 max: 100 + - name: vtol_transition_to_fw_min_airspeed_cm_s + description: "Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. Overrides `mixer_switch_trans_airspeed_cm_s` when > 0. If 0, legacy setting is used." + default_value: 0 + field: vtolTransitionToFwMinAirspeed + min: 0 + max: 20000 + - name: vtol_transition_to_mc_max_airspeed_cm_s + description: "Maximum pitot airspeed [cm/s] allowed to complete FW->MC transition when airspeed is healthy and available. If 0, FW->MC uses timer fallback." + default_value: 0 + field: vtolTransitionToMcMaxAirspeed + min: 0 + max: 20000 + - name: vtol_transition_lift_end_percent + description: "Target vertical-lift throttle scale at transition end, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + default_value: 100 + field: vtolTransitionLiftEndPercent + min: 0 + max: 100 + - name: vtol_transition_mc_authority_end_percent + description: "Target multicopter stabilization authority scale at transition end, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + default_value: 100 + field: vtolTransitionMcAuthorityEndPercent + min: 0 + max: 100 + - name: vtol_transition_fw_authority_start_percent + description: "Initial fixed-wing authority scale at transition start, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + default_value: 100 + field: vtolTransitionFwAuthorityStartPercent + min: 0 + max: 100 - name: name description: "Craft name" default_value: "" diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 47d5632762c..0fbce4deba5 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -42,7 +42,7 @@ int nextMixerProfileIndex; static bool manualTransitionModeWasActive; static bool manualTransitionReadyForEdge = true; -PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 3); void pgResetFn_mixerProfiles(mixerProfile_t *instance) { @@ -59,15 +59,10 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, .switchTransitionAirspeed = SETTING_MIXER_SWITCH_TRANS_AIRSPEED_CM_S_DEFAULT, - .vtolTransitionDynamicMixer = SETTING_VTOL_TRANSITION_DYNAMIC_MIXER_DEFAULT, - .manualVtolTransitionController = SETTING_MANUAL_VTOL_TRANSITION_CONTROLLER_DEFAULT, - .vtolTransitionToFwMinAirspeed = SETTING_VTOL_TRANSITION_TO_FW_MIN_AIRSPEED_CM_S_DEFAULT, - .vtolTransitionToMcMaxAirspeed = SETTING_VTOL_TRANSITION_TO_MC_MAX_AIRSPEED_CM_S_DEFAULT, - .vtolTransitionAirspeedTimeoutMs = SETTING_VTOL_TRANSITION_AIRSPEED_TIMEOUT_MS_DEFAULT, - .vtolTransitionScaleRampTimeMs = SETTING_VTOL_TRANSITION_SCALE_RAMP_TIME_MS_DEFAULT, - .vtolTransitionLiftEndPercent = SETTING_VTOL_TRANSITION_LIFT_END_PERCENT_DEFAULT, - .vtolTransitionMcAuthorityEndPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_END_PERCENT_DEFAULT, - .vtolTransitionFwAuthorityStartPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_START_PERCENT_DEFAULT, + .vtolTransitionDynamicMixer = SETTING_MIXER_VTOL_TRANSITION_DYNAMIC_MIXER_DEFAULT, + .manualVtolTransitionController = SETTING_MIXER_VTOL_MANUALSWITCH_AUTOTRANSITION_CONTROLLER_DEFAULT, + .vtolTransitionAirspeedTimeoutMs = SETTING_MIXER_VTOL_TRANSITION_AIRSPEED_TIMEOUT_MS_DEFAULT, + .vtolTransitionScaleRampTimeMs = SETTING_MIXER_VTOL_TRANSITION_SCALE_RAMP_TIME_MS_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, @@ -226,14 +221,14 @@ static bool hasTrustedPitotAirspeed(float *airspeedCmS) static uint16_t getAirspeedThresholdForDirection(const mixerProfileATDirection_e direction) { if (direction == MIXERAT_DIRECTION_TO_FW) { - if (currentMixerConfig.vtolTransitionToFwMinAirspeed > 0) { - return currentMixerConfig.vtolTransitionToFwMinAirspeed; + if (systemConfig()->vtolTransitionToFwMinAirspeed > 0) { + return systemConfig()->vtolTransitionToFwMinAirspeed; } return currentMixerConfig.switchTransitionAirspeed; } if (direction == MIXERAT_DIRECTION_TO_MC) { - return currentMixerConfig.vtolTransitionToMcMaxAirspeed; + return systemConfig()->vtolTransitionToMcMaxAirspeed; } return 0; @@ -250,9 +245,9 @@ static void updateTransitionScales(void) return; } - const float liftFloor = constrainf(currentMixerConfig.vtolTransitionLiftEndPercent / 100.0f, 0.0f, 1.0f); - const float mcFloor = constrainf(currentMixerConfig.vtolTransitionMcAuthorityEndPercent / 100.0f, 0.0f, 1.0f); - const float fwFloor = constrainf(currentMixerConfig.vtolTransitionFwAuthorityStartPercent / 100.0f, 0.0f, 1.0f); + const float liftFloor = constrainf(systemConfig()->vtolTransitionLiftEndPercent / 100.0f, 0.0f, 1.0f); + const float mcFloor = constrainf(systemConfig()->vtolTransitionMcAuthorityEndPercent / 100.0f, 0.0f, 1.0f); + const float fwFloor = constrainf(systemConfig()->vtolTransitionFwAuthorityStartPercent / 100.0f, 0.0f, 1.0f); const float scaleProgress = getScalingProgress(); if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 784463eba7d..1107ca030aa 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -21,13 +21,8 @@ typedef struct mixerConfig_s { uint16_t switchTransitionAirspeed; bool vtolTransitionDynamicMixer; bool manualVtolTransitionController; - uint16_t vtolTransitionToFwMinAirspeed; - uint16_t vtolTransitionToMcMaxAirspeed; uint16_t vtolTransitionAirspeedTimeoutMs; uint16_t vtolTransitionScaleRampTimeMs; - uint8_t vtolTransitionLiftEndPercent; - uint8_t vtolTransitionMcAuthorityEndPercent; - uint8_t vtolTransitionFwAuthorityStartPercent; bool tailsitterOrientationOffset; int16_t transition_PID_mmix_multiplier_roll; int16_t transition_PID_mmix_multiplier_pitch; From 587460e90a0cf848cf5ff30a9b7d5e3d96c841ac Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Sun, 17 May 2026 12:14:50 +0300 Subject: [PATCH 08/42] vtol: clean transition state internals and clarify manual switch setup - remove unused automated-transition artifacts: - drop MIXERAT_PHASE_DONE - drop unused mixerProfileAT fields (transitionInputMixing, transitionStabEndTime, transitionTransEndTime) - fix transition finalize ordering: - apply final progress/scaling before profile hot-switch - avoid final scale computation using post-switch mixer profile config - document airspeed-timeout behavior explicitly: - mixer_vtol_transition_airspeed_timeout_ms applies only in trusted pitot (airspeed-controlled) path - timer fallback path uses mixer_switch_trans_timer when pitot is unavailable/unhealthy - update VTOL and MixerProfile docs with practical test presets: - three English test profiles for VTOL ~1.0m wingspan / ~1720g AUW - legacy-compatible baseline - airspeed-first dynamic scaling - mission-authorized transition integration - add explicit safety guidance for manual RC setup: - require dedicated 3-position switch mapping - warn that overlapping MIXER PROFILE 2 and MIXER TRANSITION modes can cause order-dependent, unpredictable behavior --- docs/MixerProfile.md | 68 +++++++++++++++++++++++- docs/VTOL.md | 94 +++++++++++++++++++++++++++++++-- src/main/flight/mixer_profile.c | 7 +-- src/main/flight/mixer_profile.h | 4 -- 4 files changed, 158 insertions(+), 15 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 60f103eb6e4..11ee50b7494 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -33,6 +33,15 @@ The use of Transition Mode is recommended to enable further features and future This edge-triggered behavior is enabled by `mixer_vtol_manualswitch_autotransition_controller`. When `mixer_vtol_manualswitch_autotransition_controller = OFF`, manual transition keeps legacy behavior. +Recommended switch topology (explicit): +- Use a dedicated 3-position mapping: + - Pos1 = MC (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) + - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) + - Pos3 = FW (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) +- Avoid overlapping FW selection and transition trigger in the same position. +- Avoid 2-position setups where one position activates both `MIXER PROFILE 2` and `MIXER TRANSITION`. +- Overlapping mode activation can produce order-dependent behavior (direct profile switch path vs transition-controller path), which is unpredictable and not recommended. + ## Servo `Mixer Transition` is the input source for transition input; use this to tilt motor to gain airspeed. @@ -105,6 +114,8 @@ Ground speed is not used for transition completion/progress. Optional safety timeout: - `mixer_vtol_transition_airspeed_timeout_ms` can abort transition if airspeed condition is not met in time. +- This timeout is only active while transition completion is using trusted pitot airspeed. +- If pitot is unavailable/unhealthy, transition completion falls back to `mixer_switch_trans_timer`; timeout does not force abort in that fallback path. ### Dynamic scaling (optional) @@ -179,11 +190,66 @@ Mission path uses the same controller and completion logic as manual transition 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. Mission VTOL transition still relies on normal profile-switch infrastructure: configure two mixer profiles and a valid `MIXER PROFILE 2` mode activation condition. -Example smooth-start values (optional tuning baseline): +### Example test presets (VTOL ~1.0m wingspan, ~1720g AUW) + +These are practical starting points for first validation flights. They are examples, not universal defaults. + +#### Test 1 - Legacy-compatible baseline (manual transition check) + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = OFF` +- `set mixer_switch_trans_timer = 45` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 0` +- `set mixer_switch_trans_airspeed_cm_s = 0` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 900` +- `set mixer_vtol_transition_airspeed_timeout_ms = 0` +- `set mixer_vtol_transition_scale_ramp_time_ms = 0` +- `set nav_vtol_mission_transition_user_action = OFF` + +Behavior: +- Preserves legacy-style transition mixing while still using the new controller path. +- Useful as a known-safe baseline before enabling dynamic scaling. + +#### Test 2 - Airspeed-first + dynamic scaling (manual tuning) + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` - `set mixer_vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` +- `set mixer_switch_trans_timer = 50` +- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` +- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` - `set vtol_transition_lift_end_percent = 30` - `set vtol_transition_mc_authority_end_percent = 20` - `set vtol_transition_fw_authority_start_percent = 20` +- `set nav_vtol_mission_transition_user_action = OFF` + +Behavior: +- Uses pitot-first completion logic with timer fallback only when pitot is unavailable/unhealthy. +- Adds fast, smooth pusher/lift/authority ramping to reduce abrupt transitions. + +#### Test 3 - Mission-authorized transition (mission integration) + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` +- `set mixer_switch_trans_timer = 50` +- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` +- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `set vtol_transition_lift_end_percent = 30` +- `set vtol_transition_mc_authority_end_percent = 20` +- `set vtol_transition_fw_authority_start_percent = 20` +- `set nav_vtol_mission_transition_user_action = USER1` +- `set nav_vtol_mission_transition_min_altitude_cm = 1200` +- `set nav_vtol_mission_transition_track_distance_cm = 4000` + +Behavior: +- Uses USER1 as per-waypoint absolute target selector (clear=MC, set=FW). +- Pauses mission progression during transition and resumes only after transition completion. ### Validation Matrix (PR / SITL / HITL) diff --git a/docs/VTOL.md b/docs/VTOL.md index d3e4c96ab40..d8be0cad35e 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -324,6 +324,15 @@ Operational example: - fly in MC (pos1) -> move to Transition (pos2) to start automatic MC->FW transition -> after completion move to FW (pos3), - reverse order for FW->MC. +Important RC mapping constraint: +- Use a dedicated 3-position mapping where: + - Pos1 = MC (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) + - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) + - Pos3 = FW (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) +- Do not overlap/merge FW selection and transition trigger in the same switch position. +- Do not use a 2-position mapping where one position enables both `MIXER PROFILE 2` and `MIXER TRANSITION`. +- Mixing these mode conditions can cause race/order-dependent behavior (direct profile switch versus transition state machine), which is unpredictable in flight. + ### Mission-authorized transition semantics Mission transition is configured with `nav_vtol_mission_transition_user_action`. @@ -351,6 +360,8 @@ FW -> MC: Timeout: - `mixer_vtol_transition_airspeed_timeout_ms` can abort transition if condition is not achieved in time. +- This timeout is applied only while the transition is airspeed-controlled (trusted pitot in use). +- If pitot becomes unavailable/unhealthy, completion falls back to `mixer_switch_trans_timer` and this timeout no longer drives the decision. ### Dynamic mixer scaling @@ -375,11 +386,84 @@ Result: - transition completion still follows airspeed thresholds when pitot is healthy, - if pitot is unavailable/unhealthy, completion fallback still uses 5s. -Optional smooth-start baseline: -- `mixer_vtol_transition_dynamic_mixer = ON` -- `vtol_transition_lift_end_percent = 30` -- `vtol_transition_mc_authority_end_percent = 20` -- `vtol_transition_fw_authority_start_percent = 20` +### Example test presets (VTOL ~1.0m wingspan, ~1720g AUW) + +These are example starting points for initial testing. They are not universal values; tune after bench tests and short flight tests. + +#### Test 1 - Legacy-compatible baseline (manual transition check) + +Goal: +- Verify that the new controller does not change legacy behavior when dynamic scaling is disabled. +- Good first test after flashing. + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = OFF` +- `set mixer_switch_trans_timer = 45` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 0` +- `set mixer_switch_trans_airspeed_cm_s = 0` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 900` +- `set mixer_vtol_transition_airspeed_timeout_ms = 0` +- `set mixer_vtol_transition_scale_ramp_time_ms = 0` +- `set nav_vtol_mission_transition_user_action = OFF` + +What this does: +- Keeps transition mixing behavior close to legacy mode. +- Uses timer-driven completion when no trusted pitot threshold is configured. +- Uses conservative FW->MC completion threshold. +- Disables mission-authorized transition while validating manual behavior. + +#### Test 2 - Airspeed-first + dynamic scaling (manual transition tuning) + +Goal: +- Enable the full new behavior: airspeed-first completion and smooth authority/pusher scaling. + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` +- `set mixer_switch_trans_timer = 50` +- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` +- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `set vtol_transition_lift_end_percent = 30` +- `set vtol_transition_mc_authority_end_percent = 20` +- `set vtol_transition_fw_authority_start_percent = 20` +- `set nav_vtol_mission_transition_user_action = OFF` + +What this does: +- MC->FW completes primarily on pitot airspeed (1300 cm/s), with timer fallback only if pitot is unavailable/unhealthy. +- FW->MC completes when airspeed drops to 850 cm/s. +- Scaling ramps quickly (1.2 s) to reduce step torque and abrupt authority handoff. +- Timeout abort protects against staying too long in airspeed-controlled transition without reaching threshold. + +#### Test 3 - Mission-authorized transition (end-to-end mission flow) + +Goal: +- Validate mission User Action integration and pause/resume behavior. + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` +- `set mixer_switch_trans_timer = 50` +- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` +- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `set vtol_transition_lift_end_percent = 30` +- `set vtol_transition_mc_authority_end_percent = 20` +- `set vtol_transition_fw_authority_start_percent = 20` +- `set nav_vtol_mission_transition_user_action = USER1` +- `set nav_vtol_mission_transition_min_altitude_cm = 1200` +- `set nav_vtol_mission_transition_track_distance_cm = 4000` + +What this does: +- Uses USER1 as the absolute per-waypoint target selector: + - USER1 bit clear -> target MC + - USER1 bit set -> target FW +- Pauses mission progression during transition and resumes after completion. +- Uses straight MC->FW acceleration segment (no loiter) with a 40 m transition track distance. +- Adds a minimum altitude gate (12 m) before mission transition starts. ### Detailed effect of the three percentage settings diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 0fbce4deba5..504fcaa918a 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -119,11 +119,8 @@ void mixerConfigInit(void) void setMixerProfileAT(void) { const timeMs_t now = millis(); - const uint32_t transitionDurationMs = MAX(0, currentMixerConfig.switchTransitionTimer) * 100; mixerProfileAT.transitionStartTime = now; - mixerProfileAT.transitionStabEndTime = now; - mixerProfileAT.transitionTransEndTime = now + transitionDurationMs; mixerProfileAT.aborted = false; mixerProfileAT.hotSwitchDone = false; mixerProfileAT.usedAirspeed = false; @@ -421,13 +418,13 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) if (mixerATReadyForHotSwitch(mixerProfileAT.request)) { isMixerTransitionMixing_requested = false; + mixerProfileAT.progress = 1.0f; + updateTransitionScales(); if (!outputProfileHotSwitch(nextMixerProfileIndex)) { abortTransition(); return true; } mixerProfileAT.hotSwitchDone = true; - mixerProfileAT.progress = 1.0f; - updateTransitionScales(); mixerProfileAT.phase = MIXERAT_PHASE_IDLE; mixerProfileAT.request = MIXERAT_REQUEST_NONE; mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 1107ca030aa..f5eec0ff9db 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -57,14 +57,12 @@ typedef enum { MIXERAT_PHASE_IDLE, MIXERAT_PHASE_TRANSITION_INITIALIZE, MIXERAT_PHASE_TRANSITIONING, - MIXERAT_PHASE_DONE, } mixerProfileATState_e; typedef struct mixerProfileAT_s { mixerProfileATState_e phase; mixerProfileATDirection_e direction; mixerProfileATRequest_e request; - bool transitionInputMixing; bool aborted; bool hotSwitchDone; bool usedAirspeed; @@ -77,8 +75,6 @@ typedef struct mixerProfileAT_s { float mcAuthorityScale; float fwAuthorityScale; timeMs_t transitionStartTime; - timeMs_t transitionStabEndTime; - timeMs_t transitionTransEndTime; } mixerProfileAT_t; extern mixerProfileAT_t mixerProfileAT; bool checkMixerATRequired(mixerProfileATRequest_e required_action); From a73bc707a2c04ed3eaae74027d349995f7acc497 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Sun, 17 May 2026 12:31:26 +0300 Subject: [PATCH 09/42] vtol: harden transition controller docs and per-loop mode handling - recompute manual transition-controller enable flag after potential direct MIXER PROFILE 2 hot-switch in outputProfileUpdateTask(), so per-profile manual-controller config is applied with current profile context --- docs/MixerProfile.md | 5 +++++ docs/VTOL.md | 3 +++ src/main/flight/mixer_profile.c | 5 ++++- 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 11ee50b7494..e222b72c7a6 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -33,6 +33,10 @@ The use of Transition Mode is recommended to enable further features and future This edge-triggered behavior is enabled by `mixer_vtol_manualswitch_autotransition_controller`. When `mixer_vtol_manualswitch_autotransition_controller = OFF`, manual transition keeps legacy behavior. +Important path split: +- `MIXER PROFILE 2` remains a direct manual profile-switch path. +- Smooth VTOL transition state-machine behavior is triggered by `MIXER TRANSITION` when `mixer_vtol_manualswitch_autotransition_controller = ON`. + Recommended switch topology (explicit): - Use a dedicated 3-position mapping: - Pos1 = MC (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) @@ -116,6 +120,7 @@ Optional safety timeout: - `mixer_vtol_transition_airspeed_timeout_ms` can abort transition if airspeed condition is not met in time. - This timeout is only active while transition completion is using trusted pitot airspeed. - If pitot is unavailable/unhealthy, transition completion falls back to `mixer_switch_trans_timer`; timeout does not force abort in that fallback path. +- For airspeed-first setups, configure non-zero `mixer_switch_trans_timer` fallback (typical `40..60`, i.e. `4..6s`) so pitot-loss fallback does not complete immediately. ### Dynamic scaling (optional) diff --git a/docs/VTOL.md b/docs/VTOL.md index d8be0cad35e..2886fb87abf 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -299,6 +299,8 @@ This keeps one safety boundary for profile hot-switching and avoids separate tra - Ground speed is not used for transition completion. - Mission transition uses the same controller and does not directly manipulate motors. - Manual `MIXER PROFILE` / `MIXER TRANSITION` bypass during normal waypoint navigation is still blocked. +- `MIXER PROFILE 2` remains a direct profile-switch path when used manually. +- Smooth/automatic transition behavior is triggered by `MIXER TRANSITION` (with manual auto-controller ON) or by mission-authorized transition requests. ### Manual transition semantics @@ -362,6 +364,7 @@ Timeout: - `mixer_vtol_transition_airspeed_timeout_ms` can abort transition if condition is not achieved in time. - This timeout is applied only while the transition is airspeed-controlled (trusted pitot in use). - If pitot becomes unavailable/unhealthy, completion falls back to `mixer_switch_trans_timer` and this timeout no longer drives the decision. +- For airspeed-first setups, configure a non-zero `mixer_switch_trans_timer` fallback (typical: `40..60`, i.e. `4..6s`) to avoid immediate fallback completion when pitot is unavailable and timer fallback becomes active. ### Dynamic mixer scaling diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 504fcaa918a..efcfd4b2b78 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -469,7 +469,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) const bool manualTransitionAllowed = (posControl.navState == NAV_STATE_IDLE) || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS); const bool missionActive = (navGetCurrentStateFlags() & NAV_AUTO_WP) != 0; - const bool manualControllerEnabled = currentMixerConfig.manualVtolTransitionController && !missionActive; + bool manualControllerEnabled = false; if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { abortTransition(); @@ -484,6 +484,9 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) } } + // Recompute after potential direct profile hot-switch because this flag is per-mixer-profile. + manualControllerEnabled = currentMixerConfig.manualVtolTransitionController && !missionActive; + if (!manualControllerEnabled) { // Backward-compatible manual path: level-controlled transition mixing request. if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) { From 421a55c8d2e111d11ee4d164dbc6eb4c1761a01b Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Sun, 17 May 2026 12:52:49 +0300 Subject: [PATCH 10/42] vtol: add dedicated transition debug mode and clarify transition paths - add new debug mode `VTOL_TRANSITION` - extend debug enum with `DEBUG_VTOL_TRANSITION` - register CLI/debug name `VTOL_TRANSITION` - add settings table entry for `debug_mode` - instrument VTOL transition controller in mixer_profile task loop: - debug[0] = phase - debug[1] = request - debug[2] = direction - debug[3] = progress x1000 - debug[4] = pusherScale x1000 - debug[5] = liftScale x1000 - debug[6] = blendToFw x1000 - debug[7] = flags bitfield (active, usedAirspeed, hotSwitchDone, aborted) - improve controller consistency: - recompute manual transition-controller enable flag after potential direct MIXER PROFILE 2 hot-switch so per-profile setting is evaluated in current context - docs updates (VTOL/MixerProfile): - clarify direct `MIXER PROFILE 2` path vs controller-driven `MIXER TRANSITION` path - document airspeed-timeout scope (airspeed-controlled path only) - recommend non-zero `mixer_switch_trans_timer` fallback for airspeed-first setups - add explicit 3-position switch mapping warning and note that overlapping PROFILE2/TRANSITION activation is order-dependent and unpredictable - add VTOL transition debug mode usage and channel map - `docs/Settings.md` after debug mode table update --- docs/MixerProfile.md | 25 ++++++++++++++++++++++++- docs/Settings.md | 1 + src/main/build/debug.h | 1 + src/main/fc/cli.c | 3 ++- src/main/fc/settings.yaml | 2 +- src/main/flight/mixer_profile.c | 18 ++++++++++++++++++ 6 files changed, 47 insertions(+), 3 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index e222b72c7a6..30a2f43a6b7 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -99,7 +99,8 @@ When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Mod ### Unified VTOL transition controller Manual `MIXER TRANSITION` and mission-authorized VTOL transition both use the same internal transition controller. -This controller always computes transition progress/completion and performs profile hot-switch only inside the authorized transition state. +This controller always computes transition progress/completion and performs its own profile hot-switch only inside the authorized transition state. +Direct manual `MIXER PROFILE 2` switching remains a separate path when no transition controller path is active. When `mixer_vtol_transition_dynamic_mixer = ON`, pusher/lift/authority scaling is enabled and is driven by: - transition progress (default), or - `mixer_vtol_transition_scale_ramp_time_ms` when configured (>0). @@ -268,6 +269,28 @@ Behavior: - Mission transition with selected USER bit = `0` (TO_MC). - Failsafe/disarm during active transition (abort and no blind mission resume). +### VTOL transition debug mode (Blackbox / OSD debug) + +For transition troubleshooting, use: + +- `set debug_mode = VTOL_TRANSITION` +- `save` + +Debug channels: + +- `debug[0]` = transition phase (`0=IDLE`, `1=TRANSITION_INITIALIZE`, `2=TRANSITIONING`) +- `debug[1]` = active request (`MIXERAT_REQUEST_*` enum value) +- `debug[2]` = direction (`0=NONE`, `1=TO_FW`, `2=TO_MC`) +- `debug[3]` = progress x1000 (`0..1000`) +- `debug[4]` = pusher scale x1000 (`0..1000`) +- `debug[5]` = lift scale x1000 (`0..1000`) +- `debug[6]` = FW blend/authority scale x1000 (`0..1000`) +- `debug[7]` = flags bitfield: + - bit0: transition active + - bit1: airspeed-controlled path in use + - bit2: hot-switch done + - bit3: transition aborted + ## 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/Settings.md b/docs/Settings.md index 7d7c0e06f18..189cbb36eaa 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -782,6 +782,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) | LULU | | | SBUS2 | | | OSD_REFRESH | | +| VTOL_TRANSITION | | --- diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b33868af8b2..8e585d299ab 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -80,6 +80,7 @@ typedef enum { DEBUG_LULU, DEBUG_SBUS2, DEBUG_OSD_REFRESH, + DEBUG_VTOL_TRANSITION, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6f809432648..44845cd313f 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -223,7 +223,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "GPS", "LULU", "SBUS2", - "OSD_REFRESH" + "OSD_REFRESH", + "VTOL_TRANSITION" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 24c64b88f53..b355b7b1969 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,7 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", - "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH"] + "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "VTOL_TRANSITION"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index efcfd4b2b78..881024e7608 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -32,6 +32,7 @@ #include "navigation/navigation.h" #include "common/log.h" +#include "build/debug.h" mixerConfig_t currentMixerConfig; int currentMixerProfileIndex; @@ -535,6 +536,23 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) if (!isMixerTransitionMixing) { resetTransitionScales(); } + + // VTOL transition debug channels (DEBUG_VTOL_TRANSITION): + // [0] phase, [1] request, [2] direction, [3] progress x1000, + // [4] pusherScale x1000, [5] liftScale x1000, [6] fwBlend x1000, + // [7] flags bitfield: bit0 active, bit1 usedAirspeed, bit2 hotSwitchDone, bit3 aborted + DEBUG_SET(DEBUG_VTOL_TRANSITION, 0, mixerProfileAT.phase); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, mixerProfileAT.request); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 2, mixerProfileAT.direction); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 3, lrintf(constrainf(mixerProfileAT.progress, 0.0f, 1.0f) * 1000.0f)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 4, lrintf(constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f) * 1000.0f)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 5, lrintf(constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) * 1000.0f)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 6, lrintf(constrainf(mixerProfileAT.blendToFw, 0.0f, 1.0f) * 1000.0f)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, + (mixerATIsActive() ? 1 : 0) | + (mixerProfileAT.usedAirspeed ? 1 << 1 : 0) | + (mixerProfileAT.hotSwitchDone ? 1 << 2 : 0) | + (mixerProfileAT.aborted ? 1 << 3 : 0)); } bool mixerATIsActive(void) From 73707a95d4ed202a2ea2e44d985d1ad7f5aa11a1 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Sun, 17 May 2026 13:35:04 +0300 Subject: [PATCH 11/42] - capture debug values before transition scale cleanup so final successful transition frames remain visible in Blackbox - document VTOL debug mode usage and debug channel meanings --- docs/MixerProfile.md | 10 +++++----- src/main/flight/mixer_profile.c | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 30a2f43a6b7..7e5b802384c 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -284,12 +284,12 @@ Debug channels: - `debug[3]` = progress x1000 (`0..1000`) - `debug[4]` = pusher scale x1000 (`0..1000`) - `debug[5]` = lift scale x1000 (`0..1000`) -- `debug[6]` = FW blend/authority scale x1000 (`0..1000`) +- `debug[6]` = FW authority/blend scale x1000 (`0..1000`) - `debug[7]` = flags bitfield: - - bit0: transition active - - bit1: airspeed-controlled path in use - - bit2: hot-switch done - - bit3: transition aborted + - bit0: transition active + - bit1: airspeed-controlled path in use + - bit2: hot-switch done + - bit3: transition aborted ## 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/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 881024e7608..d330d931949 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -533,13 +533,9 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); - if (!isMixerTransitionMixing) { - resetTransitionScales(); - } - // VTOL transition debug channels (DEBUG_VTOL_TRANSITION): // [0] phase, [1] request, [2] direction, [3] progress x1000, - // [4] pusherScale x1000, [5] liftScale x1000, [6] fwBlend x1000, + // [4] pusherScale x1000, [5] liftScale x1000, [6] fwAuthority/blend x1000, // [7] flags bitfield: bit0 active, bit1 usedAirspeed, bit2 hotSwitchDone, bit3 aborted DEBUG_SET(DEBUG_VTOL_TRANSITION, 0, mixerProfileAT.phase); DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, mixerProfileAT.request); @@ -553,6 +549,10 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) (mixerProfileAT.usedAirspeed ? 1 << 1 : 0) | (mixerProfileAT.hotSwitchDone ? 1 << 2 : 0) | (mixerProfileAT.aborted ? 1 << 3 : 0)); + + if (!isMixerTransitionMixing) { + resetTransitionScales(); + } } bool mixerATIsActive(void) From 762c3103b40ddf6c6cc290019b17e9e3b6e16ac9 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Mon, 18 May 2026 20:12:50 +0300 Subject: [PATCH 12/42] fix(vtol): report actual MIXER TRANSITION/MIXER PROFILE 2 activity in mode flags - Report MIXER TRANSITION from internal transition activity when manual auto-transition controller is enabled - Report MIXER PROFILE 2 from the currently active mixer profile (not raw RC request) - Preserve legacy MIXER TRANSITION reporting when manual auto-transition controller is disabled --- docs/MixerProfile.md | 2 ++ src/main/blackbox/blackbox.c | 20 ++++++++++++++++++-- src/main/fc/fc_msp_box.c | 4 ++-- src/main/flight/mixer_profile.c | 24 ++++++++++++++++++++++++ src/main/flight/mixer_profile.h | 2 ++ 5 files changed, 48 insertions(+), 4 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 7e5b802384c..58eed8050d4 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -32,6 +32,8 @@ The use of Transition Mode is recommended to enable further features and future This edge-triggered behavior is enabled by `mixer_vtol_manualswitch_autotransition_controller`. When `mixer_vtol_manualswitch_autotransition_controller = OFF`, manual transition keeps legacy behavior. +With manual auto-transition enabled, Active Modes `MIXER TRANSITION` now indicates that the internal transition controller/mixing is actually active, not merely that the RC `MIXER TRANSITION` switch is active. +Active Modes `MIXER PROFILE 2` indicates the currently active mixer profile. Important path split: - `MIXER PROFILE 2` remains a direct manual profile-switch path. diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f5225ca3e1e..7ad94f88263 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -59,6 +59,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" #include "flight/rpm_filter.h" @@ -1366,10 +1367,25 @@ static void writeSlowFrame(void) */ static void loadSlowState(blackboxSlowState_t *slow) { + boxBitmask_t reportedRcModeFlags = rcModeActivationMask; + slow->activeWpNumber = getActiveWpNumber(); - slow->rcModeFlags = rcModeActivationMask.bits[0]; // first 32 bits of boxId_e - slow->rcModeFlags2 = rcModeActivationMask.bits[1]; // remaining bits of boxId_e + // Keep these two mode bits aligned with actual VTOL state/profile activity for status reporting. + if (isMixerProfile2ModeReportedActive()) { + bitArraySet(reportedRcModeFlags.bits, BOXMIXERPROFILE); + } else { + bitArrayClr(reportedRcModeFlags.bits, BOXMIXERPROFILE); + } + + if (isMixerTransitionModeReportedActive()) { + bitArraySet(reportedRcModeFlags.bits, BOXMIXERTRANSITION); + } else { + bitArrayClr(reportedRcModeFlags.bits, BOXMIXERTRANSITION); + } + + slow->rcModeFlags = reportedRcModeFlags.bits[0]; // first 32 bits of boxId_e + slow->rcModeFlags2 = reportedRcModeFlags.bits[1]; // remaining bits of boxId_e // Also log Nav auto enabled flight modes rather than just those selected by boxmode if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) { diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index a20a23e0b24..ba6f3f69593 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -447,8 +447,8 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)), BOXMULTIFUNCTION); #endif #if (MAX_MIXER_PROFILE_COUNT > 1) - CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION); + CHECK_ACTIVE_BOX(IS_ENABLED(isMixerProfile2ModeReportedActive()), BOXMIXERPROFILE); + CHECK_ACTIVE_BOX(IS_ENABLED(isMixerTransitionModeReportedActive()), BOXMIXERTRANSITION); #endif CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD); diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index d330d931949..69e721549e6 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -590,6 +590,30 @@ float mixerATGetBlendToFw(void) return constrainf(mixerProfileAT.blendToFw, 0.0f, 1.0f); } +bool isMixerProfile2ModeReportedActive(void) +{ +#if (MAX_MIXER_PROFILE_COUNT > 1) + return currentMixerProfileIndex > 0; +#else + return false; +#endif +} + +bool isMixerTransitionModeReportedActive(void) +{ + // Transition is actively running in the internal controller. + if (mixerATIsActive()) { + return true; + } + + // With manual auto-transition enabled, treat the RC box as a trigger/request only. + if (currentMixerConfig.manualVtolTransitionController) { + return false; + } + + return IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); +} + // switch mixerprofile without reboot bool outputProfileHotSwitch(int profile_index) { diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index f5eec0ff9db..2d23fa7994d 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -86,6 +86,8 @@ float mixerATGetLiftScale(void); float mixerATGetMcAuthorityScale(void); float mixerATGetFwAuthorityScale(void); float mixerATGetBlendToFw(void); +bool isMixerProfile2ModeReportedActive(void); +bool isMixerTransitionModeReportedActive(void); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; From 025852169962f092bbed2782e4fb757e7ab3b7a6 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 21 May 2026 10:05:59 +0300 Subject: [PATCH 13/42] debug data enchancment --- docs/MixerProfile.md | 27 ++++++++++++++++++------- src/main/flight/mixer_profile.c | 35 +++++++++++++++++++++++---------- 2 files changed, 45 insertions(+), 17 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 58eed8050d4..6ecf950bb6d 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -282,16 +282,29 @@ Debug channels: - `debug[0]` = transition phase (`0=IDLE`, `1=TRANSITION_INITIALIZE`, `2=TRANSITIONING`) - `debug[1]` = active request (`MIXERAT_REQUEST_*` enum value) -- `debug[2]` = direction (`0=NONE`, `1=TO_FW`, `2=TO_MC`) +- `debug[2]` = packed transition flags: + - bits 0-1: transition direction (`0=NONE`, `1=TO_FW`, `2=TO_MC`) + - bit2: auto-transition controller active + - bit3: transition mixing output active (`isMixerTransitionMixing`) + - bit4: RC `MIXERTRANSITION` mode active + - bit5: airspeed-controlled path in use + - bit6: hot-switch done + - bit7: transition aborted + - bit8: manual VTOL auto-transition controller enabled in current mixer config + - bit9: dynamic transition mixer enabled in current mixer config + - bits 10-11: current mixer profile index + - bits 12-13: next mixer profile index + - bit14: manual transition currently allowed by navigation state + - bit15: mission mode active + - bit16: transition mixing requested (`isMixerTransitionMixing_requested`) + - bit17: failsafe mode active + - bit18: manual VTOL auto-transition controller effective after mission gating + - bit19: RC `MIXERPROFILE` mode active - `debug[3]` = progress x1000 (`0..1000`) - `debug[4]` = pusher scale x1000 (`0..1000`) - `debug[5]` = lift scale x1000 (`0..1000`) -- `debug[6]` = FW authority/blend scale x1000 (`0..1000`) -- `debug[7]` = flags bitfield: - - bit0: transition active - - bit1: airspeed-controlled path in use - - bit2: hot-switch done - - bit3: transition aborted +- `debug[6]` = MC authority scale x1000 (`0..1000`) +- `debug[7]` = current mixer profile pitch transition PID multiplier (`transition_PID_mmix_multiplier_pitch`) ## 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/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 69e721549e6..65d220dcd3e 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -533,22 +533,37 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); + const uint32_t transitionDebugFlags = + ((uint32_t)mixerProfileAT.direction & 0x3U) | + (mixerATIsActive() ? 1U << 2 : 0U) | + (isMixerTransitionMixing ? 1U << 3 : 0U) | + (transitionModeActive ? 1U << 4 : 0U) | + (mixerProfileAT.usedAirspeed ? 1U << 5 : 0U) | + (mixerProfileAT.hotSwitchDone ? 1U << 6 : 0U) | + (mixerProfileAT.aborted ? 1U << 7 : 0U) | + (currentMixerConfig.manualVtolTransitionController ? 1U << 8 : 0U) | + (currentMixerConfig.vtolTransitionDynamicMixer ? 1U << 9 : 0U) | + (((uint32_t)currentMixerProfileIndex & 0x3U) << 10) | + (((uint32_t)nextMixerProfileIndex & 0x3U) << 12) | + (manualTransitionAllowed ? 1U << 14 : 0U) | + (missionActive ? 1U << 15 : 0U) | + (isMixerTransitionMixing_requested ? 1U << 16 : 0U) | + (FLIGHT_MODE(FAILSAFE_MODE) ? 1U << 17 : 0U) | + (manualControllerEnabled ? 1U << 18 : 0U) | + (IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) ? 1U << 19 : 0U); + // VTOL transition debug channels (DEBUG_VTOL_TRANSITION): - // [0] phase, [1] request, [2] direction, [3] progress x1000, - // [4] pusherScale x1000, [5] liftScale x1000, [6] fwAuthority/blend x1000, - // [7] flags bitfield: bit0 active, bit1 usedAirspeed, bit2 hotSwitchDone, bit3 aborted + // [0] phase, [1] request, [2] packed transition flags, [3] progress x1000, + // [4] pusherScale x1000, [5] liftScale x1000, [6] mcAuthorityScale x1000, + // [7] transition_PID_mmix_multiplier_pitch from currentMixerConfig DEBUG_SET(DEBUG_VTOL_TRANSITION, 0, mixerProfileAT.phase); DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, mixerProfileAT.request); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 2, mixerProfileAT.direction); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 2, (int32_t)transitionDebugFlags); DEBUG_SET(DEBUG_VTOL_TRANSITION, 3, lrintf(constrainf(mixerProfileAT.progress, 0.0f, 1.0f) * 1000.0f)); DEBUG_SET(DEBUG_VTOL_TRANSITION, 4, lrintf(constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f) * 1000.0f)); DEBUG_SET(DEBUG_VTOL_TRANSITION, 5, lrintf(constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) * 1000.0f)); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 6, lrintf(constrainf(mixerProfileAT.blendToFw, 0.0f, 1.0f) * 1000.0f)); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, - (mixerATIsActive() ? 1 : 0) | - (mixerProfileAT.usedAirspeed ? 1 << 1 : 0) | - (mixerProfileAT.hotSwitchDone ? 1 << 2 : 0) | - (mixerProfileAT.aborted ? 1 << 3 : 0)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 6, lrintf(constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f) * 1000.0f)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, currentMixerConfig.transition_PID_mmix_multiplier_pitch); if (!isMixerTransitionMixing) { resetTransitionScales(); From c2cdbfce37aee9e1be169f8a3190757163f721cc Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 28 May 2026 12:45:07 +0300 Subject: [PATCH 14/42] feat(nav): add VTOL transition retry and configurable fail actions Add global NAV settings for transition failure handling: - nav_vtol_transition_fail_action_mc_to_fw (IDLE/POSH/RTH/EMERGENCY_LANDING) - nav_vtol_transition_fail_action_fw_to_mc (IDLE/LOITER/RTH/EMERGENCY_LANDING/FORCE_SWITCH, default LOITER) - nav_vtol_transition_retry_on_airspeed_timeout Implement one-shot MC->FW retry after airspeed timeout (pitot-gated), including yaw scan/alignment. Handle fail actions in MIXERAT paths (mission and RTH), including FORCE_SWITCH fallback. Expose airspeed-timeout abort reason from mixer transition state. Regenerate settings docs. --- docs/Settings.md | 37 ++++ src/main/fc/settings.yaml | 21 +++ src/main/flight/mixer_profile.c | 22 ++- src/main/flight/mixer_profile.h | 2 + src/main/navigation/navigation.c | 306 ++++++++++++++++++++++++++++--- src/main/navigation/navigation.h | 18 ++ 6 files changed, 377 insertions(+), 29 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 189cbb36eaa..8c66d05270c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4711,6 +4711,43 @@ Selects which waypoint USER action bit (`USER1`..`USER4`) is used as mission VTO --- +### nav_vtol_transition_fail_action_fw_to_mc + +Action executed after a final FW->MC transition failure. FORCE_SWITCH attempts an immediate mixer hot-switch even after failed criteria. + +| Allowed Values | | +| --- | --- | +| IDLE | | +| LOITER | Default | +| RTH | | +| EMERGENCY_LANDING | | +| FORCE_SWITCH | | + +--- + +### nav_vtol_transition_fail_action_mc_to_fw + +Action executed after a final MC->FW transition failure (after retry logic, if enabled). + +| Allowed Values | | +| --- | --- | +| IDLE | Default | +| POSH | | +| RTH | | +| EMERGENCY_LANDING | | + +--- + +### nav_vtol_transition_retry_on_airspeed_timeout + +If ON, allows one retry for failed airspeed-gated MC->FW auto-transition (mission or RTH head-home): hold position, perform a 360deg yaw scan, align to best measured pitot airspeed heading, and retry transition once. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### nav_wp_enforce_altitude Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on. 0 = disabled, otherwise setting defines altitude capture tolerance [cm], e.g. 100 means required altitude is achieved when within 100cm of waypoint altitude setting. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b355b7b1969..b9125271452 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -174,6 +174,12 @@ tables: - name: nav_wp_user_action enum: navMissionUserAction_e values: ["OFF", "USER1", "USER2", "USER3", "USER4"] + - name: nav_vtol_transition_fail_action_mc_to_fw + enum: navVtolTransitionFailActionMcToFw_e + values: ["IDLE", "POSH", "RTH", "EMERGENCY_LANDING"] + - name: nav_vtol_transition_fail_action_fw_to_mc + enum: navVtolTransitionFailActionFwToMc_e + values: ["IDLE", "LOITER", "RTH", "EMERGENCY_LANDING", "FORCE_SWITCH"] - name: djiRssiSource values: ["RSSI", "CRSF_LQ"] enum: djiRssiSource_e @@ -2671,6 +2677,21 @@ groups: field: general.vtol_mission_transition_track_distance min: 1000 max: 500000 + - name: nav_vtol_transition_retry_on_airspeed_timeout + description: "If ON, allows one retry for failed airspeed-gated MC->FW auto-transition (mission or RTH head-home): hold position, perform a 360deg yaw scan, align to best measured pitot airspeed heading, and retry transition once." + default_value: OFF + field: general.vtol_transition_retry_on_airspeed_timeout + type: bool + - name: nav_vtol_transition_fail_action_mc_to_fw + description: "Action executed after a final MC->FW transition failure (after retry logic, if enabled)." + default_value: "IDLE" + field: general.vtol_transition_fail_action_mc_to_fw + table: nav_vtol_transition_fail_action_mc_to_fw + - name: nav_vtol_transition_fail_action_fw_to_mc + description: "Action executed after a final FW->MC transition failure. FORCE_SWITCH attempts an immediate mixer hot-switch even after failed criteria." + default_value: "LOITER" + field: general.vtol_transition_fail_action_fw_to_mc + table: nav_vtol_transition_fail_action_fw_to_mc - 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 65d220dcd3e..803de1a755a 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -123,6 +123,7 @@ void setMixerProfileAT(void) mixerProfileAT.transitionStartTime = now; mixerProfileAT.aborted = false; + mixerProfileAT.abortedByAirspeedTimeout = false; mixerProfileAT.hotSwitchDone = false; mixerProfileAT.usedAirspeed = false; mixerProfileAT.transitionStartAirspeedCaptured = false; @@ -263,15 +264,17 @@ static void updateTransitionScales(void) mixerProfileAT.blendToFw = constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); } -static void abortTransition(void) +static void abortTransition(const bool byAirspeedTimeout) { const bool wasActive = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; isMixerTransitionMixing_requested = false; mixerProfileAT.phase = MIXERAT_PHASE_IDLE; mixerProfileAT.aborted = wasActive; + mixerProfileAT.abortedByAirspeedTimeout = wasActive && byAirspeedTimeout; mixerProfileAT.hotSwitchDone = false; mixerProfileAT.request = MIXERAT_REQUEST_NONE; mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + mixerProfileAT.usedAirspeed = false; mixerProfileAT.transitionStartAirspeedCaptured = false; mixerProfileAT.transitionStartAirspeedCmS = 0.0f; resetTransitionScales(); @@ -388,7 +391,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) { reprocessState=false; if (required_action == MIXERAT_REQUEST_ABORT) { - abortTransition(); + abortTransition(false); return true; } switch (mixerProfileAT.phase) { @@ -413,7 +416,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) case MIXERAT_PHASE_TRANSITIONING: isMixerTransitionMixing_requested = true; if (required_action != MIXERAT_REQUEST_NONE && required_action != mixerProfileAT.request) { - abortTransition(); + abortTransition(false); return true; } @@ -422,7 +425,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) mixerProfileAT.progress = 1.0f; updateTransitionScales(); if (!outputProfileHotSwitch(nextMixerProfileIndex)) { - abortTransition(); + abortTransition(false); return true; } mixerProfileAT.hotSwitchDone = true; @@ -433,7 +436,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) } else if (mixerProfileAT.usedAirspeed && currentMixerConfig.vtolTransitionAirspeedTimeoutMs > 0 && (millis() - mixerProfileAT.transitionStartTime) >= currentMixerConfig.vtolTransitionAirspeedTimeoutMs) { - abortTransition(); + abortTransition(true); return true; } @@ -473,7 +476,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) bool manualControllerEnabled = false; if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { - abortTransition(); + abortTransition(false); mixerAT_inuse = false; } @@ -517,7 +520,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) mixerAT_inuse && !mixerProfileAT.hotSwitchDone && (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { - abortTransition(); + abortTransition(false); mixerAT_inuse = false; } @@ -580,6 +583,11 @@ bool mixerATWasAborted(void) return mixerProfileAT.aborted; } +bool mixerATWasAbortedByAirspeedTimeout(void) +{ + return mixerProfileAT.abortedByAirspeedTimeout; +} + float mixerATGetPusherScale(void) { return constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f); diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 2d23fa7994d..59fe4384809 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -64,6 +64,7 @@ typedef struct mixerProfileAT_s { mixerProfileATDirection_e direction; mixerProfileATRequest_e request; bool aborted; + bool abortedByAirspeedTimeout; bool hotSwitchDone; bool usedAirspeed; bool transitionStartAirspeedCaptured; @@ -81,6 +82,7 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action); bool mixerATUpdateState(mixerProfileATRequest_e required_action); bool mixerATIsActive(void); bool mixerATWasAborted(void); +bool mixerATWasAbortedByAirspeedTimeout(void); float mixerATGetPusherScale(void); float mixerATGetLiftScale(void); float mixerATGetMcAuthorityScale(void); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 1c0a09310c6..bed157d1a53 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -61,6 +61,7 @@ #include "rx/rx.h" #include "sensors/sensors.h" +#include "sensors/pitotmeter.h" #include "sensors/acceleration.h" #include "sensors/boardalignment.h" #include "sensors/battery.h" @@ -83,6 +84,11 @@ #define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec) #define FW_LAND_LOITER_ALT_TOLERANCE 150 +// One-shot MC->FW mission retry after airspeed-timeout: hold position, yaw scan, align to best pitot heading. +#define NAV_MIXERAT_RETRY_SCAN_STEP_CD DEGREES_TO_CENTIDEGREES(20) +#define NAV_MIXERAT_RETRY_HEADING_TOL_CD DEGREES_TO_CENTIDEGREES(5) +#define NAV_MIXERAT_RETRY_HEADING_SETTLE_MS 500 + /*----------------------------------------------------------- * Compatibility for home position *-----------------------------------------------------------*/ @@ -152,6 +158,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .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, + .vtol_transition_retry_on_airspeed_timeout = SETTING_NAV_VTOL_TRANSITION_RETRY_ON_AIRSPEED_TIMEOUT_DEFAULT, + .vtol_transition_fail_action_mc_to_fw = SETTING_NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_DEFAULT, + .vtol_transition_fail_action_fw_to_mc = SETTING_NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_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 @@ -277,8 +286,23 @@ typedef struct navMixerATMissionTransition_s { mixerProfileATRequest_e request; int32_t heading; bool active; + bool retryAttempted; + uint8_t retryStage; + int32_t retryScanStartHeading; + int32_t retryTargetHeading; + int32_t retryBestHeading; + int32_t retryScannedCd; + float retryBestAirspeedCmS; + timeMs_t retryStepStartTimeMs; + fpVector3_t retryHoldPos; } navMixerATMissionTransition_t; +typedef enum { + NAV_MIXERAT_RETRY_STAGE_IDLE = 0, + NAV_MIXERAT_RETRY_STAGE_SCAN, + NAV_MIXERAT_RETRY_STAGE_ALIGN, +} navMixerATRetryStage_e; + typedef enum { NAV_MISSION_VTOL_TRANSITION_NONE = 0, NAV_MISSION_VTOL_TRANSITION_CONTINUE, @@ -318,6 +342,11 @@ static bool isWaypointMissionValid(void); static void clearMissionVTOLTransitionState(void); static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const navWaypoint_t *waypoint); static void updateMissionTransitionGuidance(void); +static bool isTransitionRetryToFixedWingRequest(const mixerProfileATRequest_e request); +static bool hasAirspeedSensorForTransitionRetry(void); +static bool canRetryTransitionAfterAirspeedTimeout(const mixerProfileATRequest_e request); +static void beginMissionTransitionRetryScan(const mixerProfileATRequest_e request); +static void updateMissionTransitionRetryScan(void); void missionPlannerSetWaypoint(void); void initializeRTHSanityChecker(void); @@ -1067,6 +1096,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [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, @@ -1283,15 +1315,23 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) { navigationFSMStateFlags_t stateFlags = navFSM[state].stateFlags; + const bool mixerATState = (state == NAV_STATE_MIXERAT_INITIALIZE || state == NAV_STATE_MIXERAT_IN_PROGRESS); // 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) && + if (mixerATState && navMixerATPendingState == NAV_STATE_WAYPOINT_PRE_ACTION && navMixerATMissionTransition.active && navMixerATMissionTransition.request == MIXERAT_REQUEST_MISSION_TO_FW) { stateFlags |= NAV_CTL_POS | NAV_CTL_YAW; } + // During one-shot retry scan/alignment, hold position and command heading in MC. + if (mixerATState && + navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE && + isTransitionRetryToFixedWingRequest(navMixerATMissionTransition.request)) { + stateFlags |= NAV_CTL_POS | NAV_CTL_YAW; + } + return stateFlags; } @@ -2024,11 +2064,191 @@ static bool isMissionTransitionToMultirotorType(const flyingPlatformType_e platf platformType == PLATFORM_HELICOPTER; } +#ifdef USE_PITOT +static bool hasTrustedPitotAirspeed(float *airspeedCmS) +{ + if (!sensors(SENSOR_PITOT) || !pitotValidForAirspeed() || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE || + detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + return false; + } + + *airspeedCmS = pitot.airSpeed; + return true; +} +#else +static bool hasTrustedPitotAirspeed(float *airspeedCmS) +{ + UNUSED(airspeedCmS); + return false; +} +#endif + +static bool isTransitionRetryToFixedWingRequest(const mixerProfileATRequest_e request) +{ + return request == MIXERAT_REQUEST_MISSION_TO_FW || request == MIXERAT_REQUEST_RTH; +} + +static bool hasAirspeedSensorForTransitionRetry(void) +{ +#ifdef USE_PITOT + if (!sensors(SENSOR_PITOT) || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE || + detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + return false; + } + + return true; +#else + return false; +#endif +} + +static bool canRetryTransitionAfterAirspeedTimeout(const mixerProfileATRequest_e request) +{ + return navConfig()->general.vtol_transition_retry_on_airspeed_timeout && + isTransitionRetryToFixedWingRequest(request) && + hasAirspeedSensorForTransitionRetry(); +} + +static bool isTransitionToMultirotorRequest(const mixerProfileATRequest_e request) +{ + return request == MIXERAT_REQUEST_LAND || request == MIXERAT_REQUEST_MISSION_TO_MC; +} + +static navigationFSMEvent_t getMcToFwTransitionFailEvent(const mixerProfileATRequest_e request) +{ + switch ((navVtolTransitionFailActionMcToFw_e)navConfig()->general.vtol_transition_fail_action_mc_to_fw) { + case NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_POSH: + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + case NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_RTH: + return request == MIXERAT_REQUEST_RTH ? NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME : NAV_FSM_EVENT_SWITCH_TO_RTH; + case NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_EMERGENCY_LANDING: + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + case NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_IDLE: + default: + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } +} + +static navigationFSMEvent_t getFwToMcTransitionFailEvent(const mixerProfileATRequest_e request) +{ + switch ((navVtolTransitionFailActionFwToMc_e)navConfig()->general.vtol_transition_fail_action_fw_to_mc) { + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_RTH: + return NAV_FSM_EVENT_SWITCH_TO_RTH; + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_EMERGENCY_LANDING: + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_LOITER: + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_FORCE_SWITCH: + return request == MIXERAT_REQUEST_LAND ? NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME : NAV_FSM_EVENT_SWITCH_TO_RTH; + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_IDLE: + default: + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } +} + +static navigationFSMEvent_t getTransitionFailEvent(const mixerProfileATRequest_e request) +{ + if (isTransitionRetryToFixedWingRequest(request)) { + return getMcToFwTransitionFailEvent(request); + } + + if (isTransitionToMultirotorRequest(request)) { + return getFwToMcTransitionFailEvent(request); + } + + return NAV_FSM_EVENT_SWITCH_TO_IDLE; +} + +static bool tryForceSwitchAfterFwToMcFailure(const mixerProfileATRequest_e request) +{ + if (!isTransitionToMultirotorRequest(request)) { + return false; + } + + if ((navVtolTransitionFailActionFwToMc_e)navConfig()->general.vtol_transition_fail_action_fw_to_mc != NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_FORCE_SWITCH) { + return false; + } + + return STATE(AIRPLANE) && outputProfileHotSwitch(nextMixerProfileIndex); +} + static void clearMissionVTOLTransitionState(void) { navMixerATMissionTransition.active = false; navMixerATMissionTransition.request = MIXERAT_REQUEST_NONE; navMixerATMissionTransition.heading = posControl.actualState.yaw; + navMixerATMissionTransition.retryAttempted = false; + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + navMixerATMissionTransition.retryScanStartHeading = posControl.actualState.yaw; + navMixerATMissionTransition.retryTargetHeading = posControl.actualState.yaw; + navMixerATMissionTransition.retryBestHeading = posControl.actualState.yaw; + navMixerATMissionTransition.retryScannedCd = 0; + navMixerATMissionTransition.retryBestAirspeedCmS = -1.0f; + navMixerATMissionTransition.retryStepStartTimeMs = 0; + navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; +} + +static void beginMissionTransitionRetryScan(const mixerProfileATRequest_e request) +{ + navMixerATMissionTransition.request = request; + navMixerATMissionTransition.retryAttempted = true; + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_SCAN; + navMixerATMissionTransition.retryScanStartHeading = wrap_36000(posControl.actualState.yaw); + navMixerATMissionTransition.retryTargetHeading = navMixerATMissionTransition.retryScanStartHeading; + navMixerATMissionTransition.retryBestHeading = navMixerATMissionTransition.retryScanStartHeading; + navMixerATMissionTransition.retryScannedCd = 0; + navMixerATMissionTransition.retryBestAirspeedCmS = -1.0f; + navMixerATMissionTransition.retryStepStartTimeMs = millis(); + navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; +} + +static void updateMissionTransitionRetryScan(void) +{ + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_IDLE) { + return; + } + + const int32_t headingError = ABS(wrap_18000(navMixerATMissionTransition.retryTargetHeading - posControl.actualState.yaw)); + if (headingError > NAV_MIXERAT_RETRY_HEADING_TOL_CD) { + return; + } + + if ((millis() - navMixerATMissionTransition.retryStepStartTimeMs) < NAV_MIXERAT_RETRY_HEADING_SETTLE_MS) { + return; + } + + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_SCAN) { + float airspeedCmS = 0.0f; + if (hasTrustedPitotAirspeed(&airspeedCmS) && airspeedCmS > navMixerATMissionTransition.retryBestAirspeedCmS) { + navMixerATMissionTransition.retryBestAirspeedCmS = airspeedCmS; + navMixerATMissionTransition.retryBestHeading = navMixerATMissionTransition.retryTargetHeading; + } + + navMixerATMissionTransition.retryScannedCd += NAV_MIXERAT_RETRY_SCAN_STEP_CD; + if (navMixerATMissionTransition.retryScannedCd >= DEGREES_TO_CENTIDEGREES(360)) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_ALIGN; + navMixerATMissionTransition.retryTargetHeading = wrap_36000(navMixerATMissionTransition.retryBestHeading); + navMixerATMissionTransition.retryStepStartTimeMs = millis(); + return; + } + + navMixerATMissionTransition.retryTargetHeading = + wrap_36000(navMixerATMissionTransition.retryScanStartHeading + navMixerATMissionTransition.retryScannedCd); + navMixerATMissionTransition.retryStepStartTimeMs = millis(); + return; + } + + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_ALIGN) { + navMixerATMissionTransition.heading = wrap_36000(navMixerATMissionTransition.retryBestHeading); + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + } } static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const navWaypoint_t *waypoint) @@ -2092,6 +2312,15 @@ static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const static void updateMissionTransitionGuidance(void) { + if (navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE && + isTransitionRetryToFixedWingRequest(navMixerATMissionTransition.request) && + STATE(MULTIROTOR)) { + setDesiredPosition(&navMixerATMissionTransition.retryHoldPos, + navMixerATMissionTransition.retryTargetHeading, + NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return; + } + if (navMixerATMissionTransition.active && navMixerATMissionTransition.request == MIXERAT_REQUEST_MISSION_TO_FW && STATE(MULTIROTOR)) { @@ -2481,35 +2710,68 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav required_action = MIXERAT_REQUEST_NONE; break; } + + if (navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE) { + updateMissionTransitionRetryScan(); + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_IDLE) { + mixerATUpdateState(required_action); + } + updateMissionTransitionGuidance(); + return NAV_FSM_EVENT_NONE; + } + if (mixerATUpdateState(required_action)){ // MixerAT is done, switch to next state - const bool transitionAborted = mixerATWasAborted(); + bool transitionAborted = mixerATWasAborted(); + const bool transitionTimeout = mixerATWasAbortedByAirspeedTimeout(); + const bool missionTransitionWasActive = navMixerATMissionTransition.active; + if (transitionAborted && + transitionTimeout && + !navMixerATMissionTransition.retryAttempted && + canRetryTransitionAfterAirspeedTimeout(required_action) && + STATE(MULTIROTOR) && + ((required_action == MIXERAT_REQUEST_MISSION_TO_FW && missionTransitionWasActive) || + required_action == MIXERAT_REQUEST_RTH)) { + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + beginMissionTransitionRetryScan(required_action); + updateMissionTransitionGuidance(); + return NAV_FSM_EVENT_NONE; + } + + if (transitionAborted && tryForceSwitchAfterFwToMcFailure(required_action)) { + transitionAborted = false; + } + + navigationFSMEvent_t nextEvent = NAV_FSM_EVENT_SWITCH_TO_IDLE; + if (transitionAborted) { + nextEvent = getTransitionFailEvent(required_action); + } else { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LANDING: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + break; + case NAV_STATE_WAYPOINT_PRE_ACTION: + if (missionTransitionWasActive) { + nextEvent = NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; + } + break; + default: + break; + } + } + 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: - setupAltitudeController(); - return NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; - break; - case NAV_STATE_RTH_LANDING: - setupAltitudeController(); - return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; - break; - case NAV_STATE_WAYPOINT_PRE_ACTION: + if (nextEvent != NAV_FSM_EVENT_SWITCH_TO_IDLE) { setupAltitudeController(); - if (missionTransitionWasActive) { - return transitionAborted ? NAV_FSM_EVENT_SWITCH_TO_IDLE : NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; - } - return NAV_FSM_EVENT_SWITCH_TO_IDLE; - break; - default: - return NAV_FSM_EVENT_SWITCH_TO_IDLE; - break; } + return nextEvent; } updateMissionTransitionGuidance(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index f8c009dcd07..03fc33f3d81 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -336,6 +336,21 @@ typedef enum { NAV_MISSION_USER_ACTION_4, } navMissionUserAction_e; +typedef enum { + NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_IDLE = 0, + NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_POSH, + NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_RTH, + NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_EMERGENCY_LANDING, +} navVtolTransitionFailActionMcToFw_e; + +typedef enum { + NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_IDLE = 0, + NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_LOITER, + NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_RTH, + NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_EMERGENCY_LANDING, + NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_FORCE_SWITCH, +} navVtolTransitionFailActionFwToMc_e; + typedef enum { RTH_TRACKBACK_OFF, RTH_TRACKBACK_ON, @@ -424,6 +439,9 @@ typedef struct navConfig_s { 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 + bool vtol_transition_retry_on_airspeed_timeout; // Enables one-shot yaw-scan retry for failed airspeed-gated MC->FW auto-transition + uint8_t vtol_transition_fail_action_mc_to_fw; // Action after final MC->FW transition failure + uint8_t vtol_transition_fail_action_fw_to_mc; // Action after final FW->MC transition failure #ifdef USE_MULTI_MISSION uint8_t waypoint_multi_mission_index; // Index of mission to be loaded in multi mission entry #endif From 799995b9aa19c1984f080b96fe2eed324277d4b0 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 28 May 2026 14:11:34 +0300 Subject: [PATCH 15/42] fix(nav): harden VTOL retry scan and map FW->MC LOITER fail action to POSHOLD Add step/overall timeouts to MC->FW retry scan, fail retry when no trusted pitot sample is collected, and require valid pitot data before starting retry. Update FW->MC LOITER/FORCE_SWITCH fail-event mapping to POSHOLD_3D and align settings/docs description. --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation.c | 92 +++++++++++++++++++++++++------- 3 files changed, 75 insertions(+), 21 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 8c66d05270c..fa029d17cff 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4713,7 +4713,7 @@ Selects which waypoint USER action bit (`USER1`..`USER4`) is used as mission VTO ### nav_vtol_transition_fail_action_fw_to_mc -Action executed after a final FW->MC transition failure. FORCE_SWITCH attempts an immediate mixer hot-switch even after failed criteria. +Action executed after a final FW->MC transition failure. LOITER switches to POSHOLD hold at current position (fixed-wing loiter/orbit around current point). FORCE_SWITCH attempts an immediate mixer hot-switch even after failed criteria. | Allowed Values | | | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b9125271452..23f2daf2b9c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2688,7 +2688,7 @@ groups: field: general.vtol_transition_fail_action_mc_to_fw table: nav_vtol_transition_fail_action_mc_to_fw - name: nav_vtol_transition_fail_action_fw_to_mc - description: "Action executed after a final FW->MC transition failure. FORCE_SWITCH attempts an immediate mixer hot-switch even after failed criteria." + description: "Action executed after a final FW->MC transition failure. LOITER switches to POSHOLD hold at current position (fixed-wing loiter/orbit around current point). FORCE_SWITCH attempts an immediate mixer hot-switch even after failed criteria." default_value: "LOITER" field: general.vtol_transition_fail_action_fw_to_mc table: nav_vtol_transition_fail_action_fw_to_mc diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bed157d1a53..6704db89029 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -88,6 +88,8 @@ #define NAV_MIXERAT_RETRY_SCAN_STEP_CD DEGREES_TO_CENTIDEGREES(20) #define NAV_MIXERAT_RETRY_HEADING_TOL_CD DEGREES_TO_CENTIDEGREES(5) #define NAV_MIXERAT_RETRY_HEADING_SETTLE_MS 500 +#define NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS 6000 +#define NAV_MIXERAT_RETRY_MAX_TOTAL_MS 45000 /*----------------------------------------------------------- * Compatibility for home position @@ -293,6 +295,8 @@ typedef struct navMixerATMissionTransition_s { int32_t retryBestHeading; int32_t retryScannedCd; float retryBestAirspeedCmS; + bool retryHadTrustedAirspeedSample; + timeMs_t retryStartTimeMs; timeMs_t retryStepStartTimeMs; fpVector3_t retryHoldPos; } navMixerATMissionTransition_t; @@ -303,6 +307,12 @@ typedef enum { NAV_MIXERAT_RETRY_STAGE_ALIGN, } navMixerATRetryStage_e; +typedef enum { + NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS = 0, + NAV_MIXERAT_RETRY_SCAN_READY_TO_RETRY, + NAV_MIXERAT_RETRY_SCAN_FAILED, +} navMixerATRetryScanResult_e; + typedef enum { NAV_MISSION_VTOL_TRANSITION_NONE = 0, NAV_MISSION_VTOL_TRANSITION_CONTINUE, @@ -346,7 +356,7 @@ static bool isTransitionRetryToFixedWingRequest(const mixerProfileATRequest_e re static bool hasAirspeedSensorForTransitionRetry(void); static bool canRetryTransitionAfterAirspeedTimeout(const mixerProfileATRequest_e request); static void beginMissionTransitionRetryScan(const mixerProfileATRequest_e request); -static void updateMissionTransitionRetryScan(void); +static navMixerATRetryScanResult_e updateMissionTransitionRetryScan(void); void missionPlannerSetWaypoint(void); void initializeRTHSanityChecker(void); @@ -2095,7 +2105,7 @@ static bool isTransitionRetryToFixedWingRequest(const mixerProfileATRequest_e re static bool hasAirspeedSensorForTransitionRetry(void) { #ifdef USE_PITOT - if (!sensors(SENSOR_PITOT) || pitotHasFailed()) { + if (!sensors(SENSOR_PITOT) || !pitotValidForAirspeed() || pitotHasFailed()) { return false; } @@ -2139,6 +2149,8 @@ static navigationFSMEvent_t getMcToFwTransitionFailEvent(const mixerProfileATReq static navigationFSMEvent_t getFwToMcTransitionFailEvent(const mixerProfileATRequest_e request) { + UNUSED(request); + switch ((navVtolTransitionFailActionFwToMc_e)navConfig()->general.vtol_transition_fail_action_fw_to_mc) { case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_RTH: return NAV_FSM_EVENT_SWITCH_TO_RTH; @@ -2146,7 +2158,7 @@ static navigationFSMEvent_t getFwToMcTransitionFailEvent(const mixerProfileATReq return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_LOITER: case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_FORCE_SWITCH: - return request == MIXERAT_REQUEST_LAND ? NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME : NAV_FSM_EVENT_SWITCH_TO_RTH; + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_IDLE: default: return NAV_FSM_EVENT_SWITCH_TO_IDLE; @@ -2191,6 +2203,8 @@ static void clearMissionVTOLTransitionState(void) navMixerATMissionTransition.retryBestHeading = posControl.actualState.yaw; navMixerATMissionTransition.retryScannedCd = 0; navMixerATMissionTransition.retryBestAirspeedCmS = -1.0f; + navMixerATMissionTransition.retryHadTrustedAirspeedSample = false; + navMixerATMissionTransition.retryStartTimeMs = 0; navMixerATMissionTransition.retryStepStartTimeMs = 0; navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; } @@ -2205,50 +2219,80 @@ static void beginMissionTransitionRetryScan(const mixerProfileATRequest_e reques navMixerATMissionTransition.retryBestHeading = navMixerATMissionTransition.retryScanStartHeading; navMixerATMissionTransition.retryScannedCd = 0; navMixerATMissionTransition.retryBestAirspeedCmS = -1.0f; - navMixerATMissionTransition.retryStepStartTimeMs = millis(); + navMixerATMissionTransition.retryHadTrustedAirspeedSample = false; + navMixerATMissionTransition.retryStartTimeMs = millis(); + navMixerATMissionTransition.retryStepStartTimeMs = navMixerATMissionTransition.retryStartTimeMs; navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; } -static void updateMissionTransitionRetryScan(void) +static navMixerATRetryScanResult_e updateMissionTransitionRetryScan(void) { if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_IDLE) { - return; + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + const timeMs_t nowMs = millis(); + const bool overallTimedOut = (nowMs - navMixerATMissionTransition.retryStartTimeMs) >= NAV_MIXERAT_RETRY_MAX_TOTAL_MS; + if (overallTimedOut) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_FAILED; } const int32_t headingError = ABS(wrap_18000(navMixerATMissionTransition.retryTargetHeading - posControl.actualState.yaw)); - if (headingError > NAV_MIXERAT_RETRY_HEADING_TOL_CD) { - return; + const bool headingReached = headingError <= NAV_MIXERAT_RETRY_HEADING_TOL_CD; + const bool stepTimedOut = (nowMs - navMixerATMissionTransition.retryStepStartTimeMs) >= NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS; + + if (!headingReached && !stepTimedOut) { + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; } - if ((millis() - navMixerATMissionTransition.retryStepStartTimeMs) < NAV_MIXERAT_RETRY_HEADING_SETTLE_MS) { - return; + if (headingReached && + !stepTimedOut && + (nowMs - navMixerATMissionTransition.retryStepStartTimeMs) < NAV_MIXERAT_RETRY_HEADING_SETTLE_MS) { + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; } if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_SCAN) { float airspeedCmS = 0.0f; - if (hasTrustedPitotAirspeed(&airspeedCmS) && airspeedCmS > navMixerATMissionTransition.retryBestAirspeedCmS) { - navMixerATMissionTransition.retryBestAirspeedCmS = airspeedCmS; - navMixerATMissionTransition.retryBestHeading = navMixerATMissionTransition.retryTargetHeading; + if (hasTrustedPitotAirspeed(&airspeedCmS)) { + navMixerATMissionTransition.retryHadTrustedAirspeedSample = true; + if (airspeedCmS > navMixerATMissionTransition.retryBestAirspeedCmS) { + navMixerATMissionTransition.retryBestAirspeedCmS = airspeedCmS; + navMixerATMissionTransition.retryBestHeading = wrap_36000(posControl.actualState.yaw); + } } navMixerATMissionTransition.retryScannedCd += NAV_MIXERAT_RETRY_SCAN_STEP_CD; if (navMixerATMissionTransition.retryScannedCd >= DEGREES_TO_CENTIDEGREES(360)) { + if (!navMixerATMissionTransition.retryHadTrustedAirspeedSample) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_FAILED; + } + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_ALIGN; navMixerATMissionTransition.retryTargetHeading = wrap_36000(navMixerATMissionTransition.retryBestHeading); - navMixerATMissionTransition.retryStepStartTimeMs = millis(); - return; + navMixerATMissionTransition.retryStepStartTimeMs = nowMs; + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; } navMixerATMissionTransition.retryTargetHeading = wrap_36000(navMixerATMissionTransition.retryScanStartHeading + navMixerATMissionTransition.retryScannedCd); - navMixerATMissionTransition.retryStepStartTimeMs = millis(); - return; + navMixerATMissionTransition.retryStepStartTimeMs = nowMs; + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; } if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_ALIGN) { + if (!headingReached) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_FAILED; + } + navMixerATMissionTransition.heading = wrap_36000(navMixerATMissionTransition.retryBestHeading); navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_READY_TO_RETRY; } + + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; } static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const navWaypoint_t *waypoint) @@ -2712,9 +2756,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav } if (navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE) { - updateMissionTransitionRetryScan(); - if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_IDLE) { + const navMixerATRetryScanResult_e retryResult = updateMissionTransitionRetryScan(); + if (retryResult == NAV_MIXERAT_RETRY_SCAN_READY_TO_RETRY) { mixerATUpdateState(required_action); + } else if (retryResult == NAV_MIXERAT_RETRY_SCAN_FAILED) { + const navigationFSMEvent_t nextEvent = getTransitionFailEvent(required_action); + resetPositionController(); + resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + clearMissionVTOLTransitionState(); + if (nextEvent != NAV_FSM_EVENT_SWITCH_TO_IDLE) { + setupAltitudeController(); + } + return nextEvent; } updateMissionTransitionGuidance(); return NAV_FSM_EVENT_NONE; From 7fd7163436e6e9758b9c39779e7e00388f2d264a Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 28 May 2026 16:33:14 +0300 Subject: [PATCH 16/42] fix(vtol): gate manual profile switch during transition and improve retry settle timing Suppress direct BOXMIXERPROFILE hot-switch while manual transition trigger is active to prevent FW->MC direction regression. Bump PG_NAV_CONFIG to 9 and make retry settle timing start after heading tolerance is actually reached. --- src/main/flight/mixer_profile.c | 10 ++++++---- src/main/navigation/navigation.c | 17 +++++++++++++++-- 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 803de1a755a..2590f9255c0 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -473,22 +473,24 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) const bool manualTransitionAllowed = (posControl.navState == NAV_STATE_IDLE) || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS); const bool missionActive = (navGetCurrentStateFlags() & NAV_AUTO_WP) != 0; - bool manualControllerEnabled = false; + const bool manualControllerConfigured = currentMixerConfig.manualVtolTransitionController && !missionActive; + bool manualControllerEnabled = manualControllerConfigured; if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { abortTransition(false); mixerAT_inuse = false; } - // transition mode input for servo mix and motor mix - if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) + // For manual auto-transition control, suppress direct profile hotswitch while transition trigger is active. + const bool suppressDirectProfileSwitch = manualControllerConfigured && transitionModeActive; + if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse && !suppressDirectProfileSwitch) { if (isModeActivationConditionPresent(BOXMIXERPROFILE)){ outputProfileHotSwitch(IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1); } } - // Recompute after potential direct profile hot-switch because this flag is per-mixer-profile. + // Recompute after a potential direct profile hot-switch because this flag is per-mixer-profile. manualControllerEnabled = currentMixerConfig.manualVtolTransitionController && !missionActive; if (!manualControllerEnabled) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 6704db89029..10a9618a82a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -127,7 +127,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, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -298,6 +298,7 @@ typedef struct navMixerATMissionTransition_s { bool retryHadTrustedAirspeedSample; timeMs_t retryStartTimeMs; timeMs_t retryStepStartTimeMs; + timeMs_t retryHeadingReachedTimeMs; fpVector3_t retryHoldPos; } navMixerATMissionTransition_t; @@ -2206,6 +2207,7 @@ static void clearMissionVTOLTransitionState(void) navMixerATMissionTransition.retryHadTrustedAirspeedSample = false; navMixerATMissionTransition.retryStartTimeMs = 0; navMixerATMissionTransition.retryStepStartTimeMs = 0; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; } @@ -2222,6 +2224,7 @@ static void beginMissionTransitionRetryScan(const mixerProfileATRequest_e reques navMixerATMissionTransition.retryHadTrustedAirspeedSample = false; navMixerATMissionTransition.retryStartTimeMs = millis(); navMixerATMissionTransition.retryStepStartTimeMs = navMixerATMissionTransition.retryStartTimeMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; } @@ -2242,13 +2245,21 @@ static navMixerATRetryScanResult_e updateMissionTransitionRetryScan(void) const bool headingReached = headingError <= NAV_MIXERAT_RETRY_HEADING_TOL_CD; const bool stepTimedOut = (nowMs - navMixerATMissionTransition.retryStepStartTimeMs) >= NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS; + if (headingReached) { + if (!navMixerATMissionTransition.retryHeadingReachedTimeMs) { + navMixerATMissionTransition.retryHeadingReachedTimeMs = nowMs; + } + } else { + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + } + if (!headingReached && !stepTimedOut) { return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; } if (headingReached && !stepTimedOut && - (nowMs - navMixerATMissionTransition.retryStepStartTimeMs) < NAV_MIXERAT_RETRY_HEADING_SETTLE_MS) { + (nowMs - navMixerATMissionTransition.retryHeadingReachedTimeMs) < NAV_MIXERAT_RETRY_HEADING_SETTLE_MS) { return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; } @@ -2272,12 +2283,14 @@ static navMixerATRetryScanResult_e updateMissionTransitionRetryScan(void) navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_ALIGN; navMixerATMissionTransition.retryTargetHeading = wrap_36000(navMixerATMissionTransition.retryBestHeading); navMixerATMissionTransition.retryStepStartTimeMs = nowMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; } navMixerATMissionTransition.retryTargetHeading = wrap_36000(navMixerATMissionTransition.retryScanStartHeading + navMixerATMissionTransition.retryScannedCd); navMixerATMissionTransition.retryStepStartTimeMs = nowMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; } From 093c0ec76775d4711fe552f4380c52828597deef Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 28 May 2026 17:12:24 +0300 Subject: [PATCH 17/42] =?UTF-8?q?=D0=9F=D0=BE-=D0=B7=D0=B4=D1=80=D0=B0?= =?UTF-8?q?=D0=B2=D0=BE:=20code=20latch=20=D0=B7=D0=B0=20=E2=80=9Cmanual?= =?UTF-8?q?=20transition=20session=20active=E2=80=9D=20=D0=B4=D0=BE=20TRAN?= =?UTF-8?q?SITION=20switch=20OFF=20edge,=20=D0=B7=D0=B0=20=D0=B4=D0=B0=20?= =?UTF-8?q?=D0=BD=D0=B5=20=D1=81=D0=BC=D0=B5=D0=BD=D1=8F=D0=BC=D0=B5=20sem?= =?UTF-8?q?antics=20mid-session.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/MixerProfile.md | 2 ++ docs/Settings.md | 2 +- docs/VTOL.md | 2 ++ src/main/fc/settings.yaml | 2 +- src/main/flight/mixer_profile.c | 25 +++++++++++++++++++------ 5 files changed, 25 insertions(+), 8 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 6ecf950bb6d..6a05894a46d 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -31,6 +31,7 @@ The use of Transition Mode is recommended to enable further features and future - If switched OFF before hot-switch completes, the manual transition request is aborted. This edge-triggered behavior is enabled by `mixer_vtol_manualswitch_autotransition_controller`. +Set `mixer_vtol_manualswitch_autotransition_controller = ON` in both mixer profiles (MC and FW) used for switching to keep manual transition semantics consistent after profile hot-switch. When `mixer_vtol_manualswitch_autotransition_controller = OFF`, manual transition keeps legacy behavior. With manual auto-transition enabled, Active Modes `MIXER TRANSITION` now indicates that the internal transition controller/mixing is actually active, not merely that the RC `MIXER TRANSITION` switch is active. Active Modes `MIXER PROFILE 2` indicates the currently active mixer profile. @@ -44,6 +45,7 @@ Recommended switch topology (explicit): - Pos1 = MC (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) - Pos3 = FW (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) +- Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. - Avoid overlapping FW selection and transition trigger in the same position. - Avoid 2-position setups where one position activates both `MIXER PROFILE 2` and `MIXER TRANSITION`. - Overlapping mode activation can produce order-dependent behavior (direct profile switch path vs transition-controller path), which is unpredictable and not recommended. diff --git a/docs/Settings.md b/docs/Settings.md index fa029d17cff..ece1eb3655e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3222,7 +3222,7 @@ If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_ ### mixer_vtol_manualswitch_autotransition_controller -Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior. +Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior. For consistent manual transition semantics, enable this in both mixer profiles. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md index 2886fb87abf..f4f93eefb25 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -307,6 +307,7 @@ This keeps one safety boundary for profile hot-switching and avoids separate tra Intent: this does not replace legacy manual behavior. Legacy remains available and selectable. With `mixer_vtol_manualswitch_autotransition_controller = ON`: +- Enable this setting in both mixer profiles (MC and FW) for consistent edge-triggered behavior across profile hot-switches. - `MIXER TRANSITION` acts as an edge-triggered request. - A rising edge starts one transition. - Transition then runs autonomously to completion. @@ -331,6 +332,7 @@ Important RC mapping constraint: - Pos1 = MC (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) - Pos3 = FW (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) +- Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. - Do not overlap/merge FW selection and transition trigger in the same switch position. - Do not use a 2-position mapping where one position enables both `MIXER PROFILE 2` and `MIXER TRANSITION`. - Mixing these mode conditions can cause race/order-dependent behavior (direct profile switch versus transition state machine), which is unpredictable in flight. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 23f2daf2b9c..ef9ae02a2fb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1298,7 +1298,7 @@ groups: field: mixer_config.vtolTransitionDynamicMixer type: bool - name: mixer_vtol_manualswitch_autotransition_controller - description: "Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior." + description: "Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior. For consistent manual transition semantics, enable this in both mixer profiles." default_value: OFF field: mixer_config.manualVtolTransitionController type: bool diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 2590f9255c0..a7f01253cb0 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -42,6 +42,7 @@ mixerProfileAT_t mixerProfileAT; int nextMixerProfileIndex; static bool manualTransitionModeWasActive; static bool manualTransitionReadyForEdge = true; +static bool manualTransitionSessionLatched; PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 3); @@ -470,19 +471,29 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) bool mixerAT_inuse = mixerATIsActive(); const bool transitionModeActive = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); const bool transitionModeRisingEdge = transitionModeActive && !manualTransitionModeWasActive; + const bool transitionModeFallingEdge = !transitionModeActive && manualTransitionModeWasActive; const bool manualTransitionAllowed = (posControl.navState == NAV_STATE_IDLE) || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS); const bool missionActive = (navGetCurrentStateFlags() & NAV_AUTO_WP) != 0; const bool manualControllerConfigured = currentMixerConfig.manualVtolTransitionController && !missionActive; - bool manualControllerEnabled = manualControllerConfigured; + bool manualControllerEnabled = manualControllerConfigured || manualTransitionSessionLatched; + + if (manualControllerConfigured && transitionModeRisingEdge) { + manualTransitionSessionLatched = true; + } + + if (transitionModeFallingEdge) { + manualTransitionSessionLatched = false; + } if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { abortTransition(false); + manualTransitionSessionLatched = false; mixerAT_inuse = false; } // For manual auto-transition control, suppress direct profile hotswitch while transition trigger is active. - const bool suppressDirectProfileSwitch = manualControllerConfigured && transitionModeActive; + const bool suppressDirectProfileSwitch = manualControllerEnabled && transitionModeActive; if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse && !suppressDirectProfileSwitch) { if (isModeActivationConditionPresent(BOXMIXERPROFILE)){ @@ -491,7 +502,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) } // Recompute after a potential direct profile hot-switch because this flag is per-mixer-profile. - manualControllerEnabled = currentMixerConfig.manualVtolTransitionController && !missionActive; + manualControllerEnabled = (currentMixerConfig.manualVtolTransitionController && !missionActive) || manualTransitionSessionLatched; if (!manualControllerEnabled) { // Backward-compatible manual path: level-controlled transition mixing request. @@ -504,6 +515,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) manualTransitionReadyForEdge = true; } else { if (!transitionModeActive) { + manualTransitionSessionLatched = false; manualTransitionReadyForEdge = true; if (!mixerAT_inuse) { isMixerTransitionMixing_requested = false; @@ -555,7 +567,8 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) (isMixerTransitionMixing_requested ? 1U << 16 : 0U) | (FLIGHT_MODE(FAILSAFE_MODE) ? 1U << 17 : 0U) | (manualControllerEnabled ? 1U << 18 : 0U) | - (IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) ? 1U << 19 : 0U); + (IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) ? 1U << 19 : 0U) | + (manualTransitionSessionLatched ? 1U << 20 : 0U); // VTOL transition debug channels (DEBUG_VTOL_TRANSITION): // [0] phase, [1] request, [2] packed transition flags, [3] progress x1000, @@ -631,8 +644,8 @@ bool isMixerTransitionModeReportedActive(void) return true; } - // With manual auto-transition enabled, treat the RC box as a trigger/request only. - if (currentMixerConfig.manualVtolTransitionController) { + // With manual auto-transition enabled (or session latched), treat RC as trigger only. + if (currentMixerConfig.manualVtolTransitionController || manualTransitionSessionLatched) { return false; } From cb213fd3ea30337d858926e9f165c1d64a3f6950 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Wed, 3 Jun 2026 11:55:15 +0300 Subject: [PATCH 18/42] docs(vtol): align profile order guidance and clarify switch examples --- docs/MixerProfile.md | 31 ++++++---- docs/Navigation.md | 3 +- docs/Settings.md | 27 +++++---- docs/VTOL.md | 47 +++++++++------ src/main/fc/config.c | 3 +- src/main/fc/config.h | 1 + src/main/fc/settings.yaml | 18 +++--- src/main/flight/mixer_profile.c | 101 ++++++++++++++++++++++++++++---- src/main/flight/mixer_profile.h | 2 +- 9 files changed, 169 insertions(+), 64 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 6a05894a46d..69d543338dc 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -12,6 +12,12 @@ For VTOL setup. one mixer_profile is used for multi-rotor(MR) and the other is u By default, switching between profiles requires reboot to take affect. However, using the RC mode: `MIXER PROFILE 2` will allow in flight switching for things like VTOL operation . And will re-initialize pid and navigation controllers for current MC or FW flying mode. +For consistency, this document uses the long-standing VTOL order: +- Profile 1 = fixed-wing (FW) +- Profile 2 = multicopter (MC) + +The firmware can work with the profiles swapped, but the examples below keep this order so the switch logic is easier to follow. + Please note that this is an emerging / experimental capability that will require some effort by the pilot to implement. ## Mixer Transition input @@ -29,6 +35,8 @@ The use of Transition Mode is recommended to enable further features and future - Keeping the mode ON does not repeatedly restart transitions. - A new transition requires mode OFF then ON again. - If switched OFF before hot-switch completes, the manual transition request is aborted. +- If valid pitot is present and MC->FW threshold is configured, direct manual profile hot-switch to FW is blocked until threshold is reached. +- Optional FW protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can auto-request FW->MC transition when valid pitot airspeed drops to/below the configured value (`0` disables). This edge-triggered behavior is enabled by `mixer_vtol_manualswitch_autotransition_controller`. Set `mixer_vtol_manualswitch_autotransition_controller = ON` in both mixer profiles (MC and FW) used for switching to keep manual transition semantics consistent after profile hot-switch. @@ -40,15 +48,19 @@ Important path split: - `MIXER PROFILE 2` remains a direct manual profile-switch path. - Smooth VTOL transition state-machine behavior is triggered by `MIXER TRANSITION` when `mixer_vtol_manualswitch_autotransition_controller = ON`. -Recommended switch topology (explicit): +Recommended switch topology example (clear 3-position setup): +- This example assumes the usual VTOL order used in this document: + - Profile 1 = FW + - Profile 2 = MC - Use a dedicated 3-position mapping: - - Pos1 = MC (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) + - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) - - Pos3 = FW (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) + - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) - Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. - Avoid overlapping FW selection and transition trigger in the same position. - Avoid 2-position setups where one position activates both `MIXER PROFILE 2` and `MIXER TRANSITION`. - Overlapping mode activation can produce order-dependent behavior (direct profile switch path vs transition-controller path), which is unpredictable and not recommended. +- If you intentionally swap the profile order, the same idea still works; just swap the FW and MC end positions. ## Servo @@ -115,7 +127,6 @@ When pitot airspeed is healthy and available, transition completion uses pitot t - `vtol_transition_to_fw_min_airspeed_cm_s` for MC->FW - `vtol_transition_to_mc_max_airspeed_cm_s` for FW->MC -- If `vtol_transition_to_fw_min_airspeed_cm_s = 0`, MC->FW falls back to legacy `mixer_switch_trans_airspeed_cm_s`. If pitot is unavailable/unhealthy (or threshold is `0`), timer fallback is used (`mixer_switch_trans_timer`). Ground speed is not used for transition completion/progress. @@ -141,8 +152,9 @@ With dynamic scaling enabled, `vtol_transition_fw_authority_start_percent = 100` Optional scaling ramp timer: -- `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): scaling remains coupled to transition progress (legacy-compatible behavior). -- `mixer_vtol_transition_scale_ramp_time_ms > 0`: scaling uses this timer, while transition completion stays airspeed-first (or timer fallback if pitot unavailable/unhealthy). +- trusted pitot available/healthy: scaling follows airspeed-based transition progress. +- `mixer_vtol_transition_scale_ramp_time_ms > 0`: if trusted pitot becomes unavailable/unhealthy, scaling falls back to this timer. +- `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): if trusted pitot is unavailable/unhealthy, scaling falls back to transition progress/timer behavior. Example: @@ -150,7 +162,8 @@ Example: - `mixer_vtol_transition_scale_ramp_time_ms = 1200` Result: -- scaling reaches target levels in ~1.2s, +- when trusted pitot is healthy, scaling still follows airspeed progress, +- if trusted pitot becomes unavailable/unhealthy, scaling reaches target levels in ~1.2s, - transition completion still follows airspeed threshold when pitot is healthy, - timer fallback completion still uses 5s when pitot is unavailable/unhealthy. @@ -161,14 +174,12 @@ INAV supports mission-requested VTOL transitions through the existing automated - `nav_vtol_mission_transition_user_action` (`OFF`, `USER1`, `USER2`, `USER3`, `USER4`) - `nav_vtol_mission_transition_min_altitude_cm` (optional, `0` disables minimum-altitude check) - `vtol_transition_to_fw_min_airspeed_cm_s` (preferred MC->FW threshold) -- `mixer_switch_trans_airspeed_cm_s` (legacy MC->FW fallback when preferred threshold is `0`) Scope note: - Per-mixer-profile settings: - `mixer_automated_switch` - `mixer_switch_trans_timer` - - `mixer_switch_trans_airspeed_cm_s` - `mixer_vtol_transition_dynamic_mixer` - `mixer_vtol_manualswitch_autotransition_controller` - `mixer_vtol_transition_airspeed_timeout_ms` @@ -176,6 +187,7 @@ Scope note: - Global settings: - `vtol_transition_to_fw_min_airspeed_cm_s` - `vtol_transition_to_mc_max_airspeed_cm_s` + - `vtol_fw_to_mc_auto_switch_airspeed_cm_s` - `vtol_transition_lift_end_percent` - `vtol_transition_mc_authority_end_percent` - `vtol_transition_fw_authority_start_percent` @@ -211,7 +223,6 @@ CLI: - `set mixer_vtol_transition_dynamic_mixer = OFF` - `set mixer_switch_trans_timer = 45` - `set vtol_transition_to_fw_min_airspeed_cm_s = 0` -- `set mixer_switch_trans_airspeed_cm_s = 0` - `set vtol_transition_to_mc_max_airspeed_cm_s = 900` - `set mixer_vtol_transition_airspeed_timeout_ms = 0` - `set mixer_vtol_transition_scale_ramp_time_ms = 0` diff --git a/docs/Navigation.md b/docs/Navigation.md index 9fb02f8a7a0..1a96d947d40 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -113,7 +113,6 @@ Configuration: - `nav_vtol_mission_transition_track_distance_cm` configures straight-line MC->FW transition guidance distance. - VTOL transition completion logic is shared with manual MIXER TRANSITION and uses mixer transition settings: - preferred MC->FW threshold: `vtol_transition_to_fw_min_airspeed_cm_s` - - legacy MC->FW fallback (when preferred threshold is `0`): `mixer_switch_trans_airspeed_cm_s` - FW->MC threshold: `vtol_transition_to_mc_max_airspeed_cm_s` Behavior on each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`): @@ -131,7 +130,7 @@ 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 and FW -> MC completion uses pitot airspeed thresholds when healthy/available (`vtol_transition_to_fw_min_airspeed_cm_s`, `vtol_transition_to_mc_max_airspeed_cm_s`). -- If pitot is unavailable/unhealthy (or threshold disabled), timer fallback (`mixer_switch_trans_timer`) is used. +- If pitot is unavailable/unhealthy (or threshold is `0`), timer fallback (`mixer_switch_trans_timer`) is used. - Ground speed is not used for transition progress/completion. - 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. diff --git a/docs/Settings.md b/docs/Settings.md index ece1eb3655e..d83167504f1 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3200,20 +3200,12 @@ If enabled, control_profile_index will follow mixer_profile index. Set to OFF(de --- -### mixer_switch_trans_airspeed_cm_s - -Legacy MC->FW airspeed threshold [cm/s] for automated profile switch. Used when `vtol_transition_to_fw_min_airspeed_cm_s = 0`. If airspeed is unavailable, timer-based fallback (`mixer_switch_trans_timer`) is used. - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | 10000 | - ---- - ### mixer_switch_trans_timer If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. +If trusted pitot is unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms = 0`, dynamic scaling also falls back to this transition progress/timer behavior. + | Default | Min | Max | | --- | --- | --- | | 0 | 0 | 200 | @@ -3252,7 +3244,7 @@ Enables dynamic VTOL transition progress/scaling controller shared by mission-au ### mixer_vtol_transition_scale_ramp_time_ms -Optional dynamic scaling ramp duration [ms]. When > 0 and `mixer_vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling uses this timer instead of transition completion progress. Set to 0 to keep legacy progress-coupled scaling behavior. +Optional dynamic scaling fallback ramp duration [ms]. When > 0 and `mixer_vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling still follows trusted pitot-based transition progress when available; if trusted pitot becomes unavailable/unhealthy, scaling falls back to this timer. If set to 0, scaling falls back to transition progress/timer behavior. | Default | Min | Max | | --- | --- | --- | @@ -7096,6 +7088,16 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, --- +### vtol_fw_to_mc_auto_switch_airspeed_cm_s + +Automatic FW->MC protection threshold [cm/s] used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. If set above 0 and valid pitot airspeed is at/below this value while in FW, controller requests FW->MC transition automatically. Set to 0 to disable. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20000 | + +--- + ### vtol_transition_fw_authority_start_percent Initial fixed-wing authority scale at transition start, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. @@ -7128,7 +7130,7 @@ Target multicopter stabilization authority scale at transition end, in percent. ### vtol_transition_to_fw_min_airspeed_cm_s -Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. Overrides `mixer_switch_trans_airspeed_cm_s` when > 0. If 0, legacy setting is used. +Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. If 0, MC->FW uses timer fallback (`mixer_switch_trans_timer`). | Default | Min | Max | | --- | --- | --- | @@ -7299,4 +7301,3 @@ Defines rotation rate on YAW axis that UAV will try to archive on max. stick def | 20 | 1 | 180 | --- - diff --git a/docs/VTOL.md b/docs/VTOL.md index f4f93eefb25..7221abf8400 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -23,15 +23,21 @@ We highly value your feedback as it plays a crucial role in the development and # VTOL Configuration Steps ### The VTOL functionality is achieved by switching/transitioning between two configurations stored in the FC. VTOL specific configurations are Mixer Profiles with associated control profiles. One profile set is for fixed-wing(FW) mode, One is for multi-copter(MC) mode. Configuration/Settings other than Mixer/control profiles are shared among two modes + +This guide uses the long-standing VTOL setup order: +- Profile 1 = fixed-wing (FW) +- Profile 2 = multicopter (MC) + +The firmware can work with the profiles swapped, but keeping one order in the guide makes the setup steps easier to follow. ![Alt text](Screenshots/mixerprofile_flow.png) 0. **Find a DIFF ALL file for your model and start from there if possible** - Be aware that `MIXER PROFILE 2` RC mode setting introduced by diff file can get your stuck in a mixer_profile. remove or change channel to proceed 1. **Setup Profile 1:** - - Configure it as a normal fixed-wing/multi-copter. + - Configure it as your normal fixed-wing setup. 2. **Setup Profile 2:** - - Configure it as a normal multi-copter/fixed-wing. + - Configure it as your normal multicopter setup. 3. **Mode Tab Settings:** - Set up switching in the mode tab. @@ -125,8 +131,8 @@ save save ``` -2. **Configure the fixed-wing/Multi-Copter:** - - Configure your fixed-wing/Multi-Copter as you normally would, or you can copy and paste default settings to expedite the process. +2. **Configure the fixed-wing:** + - Configure your fixed-wing as you normally would, or you can copy and paste default settings to speed things up. - Dshot esc protocol availability might be limited depends on outputs and fc board you are using. change the motor wiring or use oneshot/multishot esc protocol and calibrate throttle range. - You can use throttle = -1 as a placeholder for the motor you wish to stop if the motor isn't the last motor - Consider conducting a test flight to ensure that everything operates as expected. And tune the settings, trim the servos. @@ -147,8 +153,8 @@ You must also assign the tilting servos values using the MAX values. If you don save ``` -2. **Configure the Multicopter/tricopter:** - - Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and control_profile 2. +2. **Configure the multicopter/tricopter:** + - Set up your multicopter/tricopter as usual, this time for mixer_profile 2 and control_profile 2. - Utilize the 'MAX' input in the servo mixer to tilt the motors without altering the servo midpoint. - At this stage, focus on configuring profile-specific settings. You can streamline this process by copying and pasting the default PID settings. - you can set -1 in motor mixer throttle as a place holder: this will disable that motor but will load following the motor rules @@ -314,6 +320,8 @@ With `mixer_vtol_manualswitch_autotransition_controller = ON`: - Keeping the mode ON does not repeatedly retrigger transition. - To start another transition, mode must go OFF then ON again. - If mode is turned OFF before hot-switch, transition request is aborted safely. +- If valid pitot is present and MC->FW airspeed threshold is configured, direct manual profile hot-switch to FW is blocked until threshold is reached. +- Optional FW safety fallback: set `vtol_fw_to_mc_auto_switch_airspeed_cm_s > 0` to auto-request FW->MC transition when valid pitot airspeed drops to/below the configured value. With `mixer_vtol_manualswitch_autotransition_controller = OFF`: - legacy manual behavior is preserved for backward compatibility. @@ -357,7 +365,7 @@ For MC -> FW mission transition: MC -> FW: - completion threshold: `vtol_transition_to_fw_min_airspeed_cm_s` -- if this is `0`, legacy `mixer_switch_trans_airspeed_cm_s` is used. +- if this is `0`, MC->FW uses timer fallback (`mixer_switch_trans_timer`). FW -> MC: - completion threshold: `vtol_transition_to_mc_max_airspeed_cm_s` @@ -379,15 +387,17 @@ When `mixer_vtol_transition_dynamic_mixer = ON`, transition progress additionall When `mixer_vtol_transition_dynamic_mixer = OFF`, legacy static transition mixing behavior is preserved. Optional decoupled scaling ramp: -- `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): scaling follows transition progress (legacy-compatible behavior). -- `mixer_vtol_transition_scale_ramp_time_ms > 0`: scaling uses this ramp timer, while completion logic remains unchanged (airspeed-first; timer fallback when pitot is unavailable/unhealthy). +- trusted pitot available/healthy: scaling follows airspeed-based transition progress. +- `mixer_vtol_transition_scale_ramp_time_ms > 0`: if trusted pitot becomes unavailable/unhealthy, scaling falls back to this ramp timer. +- `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): if trusted pitot is unavailable/unhealthy, scaling falls back to transition progress/timer behavior. Example: - `mixer_switch_trans_timer = 50` (5s fallback completion timer) - `mixer_vtol_transition_scale_ramp_time_ms = 1200` Result: -- pusher/lift/authority scaling reaches target levels in ~1.2s, +- when trusted pitot is healthy, pusher/lift/authority scaling still follows airspeed progress, +- if trusted pitot becomes unavailable/unhealthy, scaling reaches target levels in ~1.2s, - transition completion still follows airspeed thresholds when pitot is healthy, - if pitot is unavailable/unhealthy, completion fallback still uses 5s. @@ -406,7 +416,6 @@ CLI: - `set mixer_vtol_transition_dynamic_mixer = OFF` - `set mixer_switch_trans_timer = 45` - `set vtol_transition_to_fw_min_airspeed_cm_s = 0` -- `set mixer_switch_trans_airspeed_cm_s = 0` - `set vtol_transition_to_mc_max_airspeed_cm_s = 900` - `set mixer_vtol_transition_airspeed_timeout_ms = 0` - `set mixer_vtol_transition_scale_ramp_time_ms = 0` @@ -511,11 +520,11 @@ The new VTOL settings are split into two groups: ### Per-mixer-profile settings -These can differ between mixer profile 1 (typically MC) and mixer profile 2 (typically FW): +In the examples in this guide, mixer profile 1 is FW and mixer profile 2 is MC. +These settings can differ between the two mixer profiles: - `mixer_automated_switch` - `mixer_switch_trans_timer` -- `mixer_switch_trans_airspeed_cm_s` - `mixer_vtol_transition_dynamic_mixer` - `mixer_vtol_manualswitch_autotransition_controller` - `mixer_vtol_transition_airspeed_timeout_ms` @@ -527,6 +536,7 @@ These are shared system-wide and are not profile-specific: - `vtol_transition_to_fw_min_airspeed_cm_s` - `vtol_transition_to_mc_max_airspeed_cm_s` +- `vtol_fw_to_mc_auto_switch_airspeed_cm_s` - `vtol_transition_lift_end_percent` - `vtol_transition_mc_authority_end_percent` - `vtol_transition_fw_authority_start_percent` @@ -547,15 +557,15 @@ Use these commands in CLI (`set ...`, then `save`): - `set vtol_transition_to_fw_min_airspeed_cm_s = ` - Preferred MC -> FW completion threshold (pitot airspeed). -- `set mixer_switch_trans_airspeed_cm_s = ` - - Legacy MC -> FW threshold, used when `vtol_transition_to_fw_min_airspeed_cm_s = 0`. - - `set mixer_switch_trans_timer = ` - Timer-based transition duration fallback (used when pitot airspeed is unavailable/unhealthy). - `set vtol_transition_to_mc_max_airspeed_cm_s = ` - FW -> MC completion threshold (pitot airspeed). +- `set vtol_fw_to_mc_auto_switch_airspeed_cm_s = ` + - Optional low-airspeed FW protection threshold for manual auto-transition controller (`0` disables). + - `set mixer_vtol_transition_airspeed_timeout_ms = ` - Transition timeout/abort window. @@ -625,8 +635,9 @@ Dynamic mixer scaling (`mixer_vtol_transition_dynamic_mixer = ON`) uses this pro - MC authority ramps `vtol_transition_mc_authority_end_percent -> 1` - FW authority ramps `1 -> vtol_transition_fw_authority_start_percent` -If `mixer_vtol_transition_scale_ramp_time_ms > 0`, dynamic scaling uses that timer-based ramp instead of transition-progress coupling. -This changes only scaling shape. Transition completion logic remains airspeed-first (with timer fallback when pitot is unavailable/unhealthy). +Dynamic scaling prefers trusted pitot-based transition progress whenever available. +If trusted pitot becomes unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms > 0`, scaling falls back to that timer-based ramp. +If trusted pitot is unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms = 0`, scaling falls back to transition progress/timer behavior (`mixer_switch_trans_timer`). For transition/pusher motors (`-2.0 < throttle < -1.0`), output is interpolated from idle to target: diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 23d62fa40ac..f5cafa62ae7 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -103,7 +103,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 9); PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, @@ -119,6 +119,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .throttle_tilt_compensation_strength = SETTING_THROTTLE_TILT_COMP_STR_DEFAULT, // 0-100, 0 - disabled .vtolTransitionToFwMinAirspeed = SETTING_VTOL_TRANSITION_TO_FW_MIN_AIRSPEED_CM_S_DEFAULT, .vtolTransitionToMcMaxAirspeed = SETTING_VTOL_TRANSITION_TO_MC_MAX_AIRSPEED_CM_S_DEFAULT, + .vtolFwToMcAutoSwitchAirspeed = SETTING_VTOL_FW_TO_MC_AUTO_SWITCH_AIRSPEED_CM_S_DEFAULT, .vtolTransitionLiftEndPercent = SETTING_VTOL_TRANSITION_LIFT_END_PERCENT_DEFAULT, .vtolTransitionMcAuthorityEndPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_END_PERCENT_DEFAULT, .vtolTransitionFwAuthorityStartPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_START_PERCENT_DEFAULT, diff --git a/src/main/fc/config.h b/src/main/fc/config.h index c7f2501fceb..1478754f350 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -80,6 +80,7 @@ typedef struct systemConfig_s { uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. uint16_t vtolTransitionToFwMinAirspeed; uint16_t vtolTransitionToMcMaxAirspeed; + uint16_t vtolFwToMcAutoSwitchAirspeed; uint8_t vtolTransitionLiftEndPercent; uint8_t vtolTransitionMcAuthorityEndPercent; uint8_t vtolTransitionFwAuthorityStartPercent; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ef9ae02a2fb..75dd54e4ae4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1281,17 +1281,11 @@ groups: field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." + description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. If trusted pitot is unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms = 0`, dynamic scaling also falls back to this transition progress/timer behavior." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 max: 200 - - name: mixer_switch_trans_airspeed_cm_s - description: "Legacy MC->FW airspeed threshold [cm/s] for automated profile switch. Used when `vtol_transition_to_fw_min_airspeed_cm_s = 0`. 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: mixer_vtol_transition_dynamic_mixer description: "Enables dynamic VTOL transition progress/scaling controller shared by mission-authorized and manual MIXER TRANSITION paths." default_value: OFF @@ -1309,7 +1303,7 @@ groups: min: 0 max: 60000 - name: mixer_vtol_transition_scale_ramp_time_ms - description: "Optional dynamic scaling ramp duration [ms]. When > 0 and `mixer_vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling uses this timer instead of transition completion progress. Set to 0 to keep legacy progress-coupled scaling behavior." + description: "Optional dynamic scaling fallback ramp duration [ms]. When > 0 and `mixer_vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling still follows trusted pitot-based transition progress when available; if trusted pitot becomes unavailable/unhealthy, scaling falls back to this timer. If set to 0, scaling falls back to transition progress/timer behavior." default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs min: 0 @@ -4027,7 +4021,7 @@ groups: min: 0 max: 100 - name: vtol_transition_to_fw_min_airspeed_cm_s - description: "Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. Overrides `mixer_switch_trans_airspeed_cm_s` when > 0. If 0, legacy setting is used." + description: "Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. If 0, MC->FW uses timer fallback (`mixer_switch_trans_timer`)." default_value: 0 field: vtolTransitionToFwMinAirspeed min: 0 @@ -4038,6 +4032,12 @@ groups: field: vtolTransitionToMcMaxAirspeed min: 0 max: 20000 + - name: vtol_fw_to_mc_auto_switch_airspeed_cm_s + description: "Automatic FW->MC protection threshold [cm/s] used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. If set above 0 and valid pitot airspeed is at/below this value while in FW, controller requests FW->MC transition automatically. Set to 0 to disable." + default_value: 0 + field: vtolFwToMcAutoSwitchAirspeed + min: 0 + max: 20000 - name: vtol_transition_lift_end_percent description: "Target vertical-lift throttle scale at transition end, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." default_value: 100 diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index a7f01253cb0..f305b57459b 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -44,7 +44,7 @@ static bool manualTransitionModeWasActive; static bool manualTransitionReadyForEdge = true; static bool manualTransitionSessionLatched; -PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 3); +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 4); void pgResetFn_mixerProfiles(mixerProfile_t *instance) { @@ -60,7 +60,6 @@ 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, .vtolTransitionDynamicMixer = SETTING_MIXER_VTOL_TRANSITION_DYNAMIC_MIXER_DEFAULT, .manualVtolTransitionController = SETTING_MIXER_VTOL_MANUALSWITCH_AUTOTRANSITION_CONTROLLER_DEFAULT, .vtolTransitionAirspeedTimeoutMs = SETTING_MIXER_VTOL_TRANSITION_AIRSPEED_TIMEOUT_MS_DEFAULT, @@ -129,6 +128,7 @@ void setMixerProfileAT(void) mixerProfileAT.usedAirspeed = false; mixerProfileAT.transitionStartAirspeedCaptured = false; mixerProfileAT.progress = 0.0f; + mixerProfileAT.scalingProgress = 0.0f; mixerProfileAT.transitionStartAirspeedCmS = 0.0f; mixerProfileAT.blendToFw = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW ? 0.0f : 1.0f; mixerProfileAT.pusherScale = 1.0f; @@ -162,6 +162,7 @@ static mixerProfileATDirection_e directionForRequest(const mixerProfileATRequest static void resetTransitionScales(void) { mixerProfileAT.progress = 0.0f; + mixerProfileAT.scalingProgress = 0.0f; mixerProfileAT.blendToFw = 0.0f; mixerProfileAT.pusherScale = 0.0f; mixerProfileAT.liftScale = 1.0f; @@ -172,6 +173,7 @@ static void resetTransitionScales(void) static void setLegacyTransitionScales(void) { mixerProfileAT.progress = 1.0f; + mixerProfileAT.scalingProgress = 1.0f; mixerProfileAT.blendToFw = 1.0f; mixerProfileAT.pusherScale = 1.0f; mixerProfileAT.liftScale = 1.0f; @@ -190,12 +192,24 @@ static float getScalingProgress(void) return 1.0f; } + if (mixerProfileAT.usedAirspeed) { + mixerProfileAT.scalingProgress = constrainf(mixerProfileAT.progress, 0.0f, 1.0f); + return mixerProfileAT.scalingProgress; + } + if (currentMixerConfig.vtolTransitionScaleRampTimeMs > 0) { const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; - return constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); + const float rampProgress = constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); + + // Preserve already-applied scaling if pitot drops out mid-transition. + mixerProfileAT.scalingProgress = MAX(mixerProfileAT.scalingProgress, rampProgress); + return mixerProfileAT.scalingProgress; } - return constrainf(mixerProfileAT.progress, 0.0f, 1.0f); + // Last-resort compatibility path: with no trusted pitot and no dedicated + // scaling ramp configured, reuse transition progress/timer behavior. + mixerProfileAT.scalingProgress = MAX(mixerProfileAT.scalingProgress, constrainf(mixerProfileAT.progress, 0.0f, 1.0f)); + return mixerProfileAT.scalingProgress; } static bool hasTrustedPitotAirspeed(float *airspeedCmS) @@ -218,13 +232,28 @@ static bool hasTrustedPitotAirspeed(float *airspeedCmS) #endif } +static bool hasPitotSensorForManualProtection(void) +{ +#ifdef USE_PITOT + if (!sensors(SENSOR_PITOT) || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE || + detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + return false; + } + + return true; +#else + return false; +#endif +} + static uint16_t getAirspeedThresholdForDirection(const mixerProfileATDirection_e direction) { if (direction == MIXERAT_DIRECTION_TO_FW) { - if (systemConfig()->vtolTransitionToFwMinAirspeed > 0) { - return systemConfig()->vtolTransitionToFwMinAirspeed; - } - return currentMixerConfig.switchTransitionAirspeed; + return systemConfig()->vtolTransitionToFwMinAirspeed; } if (direction == MIXERAT_DIRECTION_TO_MC) { @@ -234,6 +263,48 @@ static uint16_t getAirspeedThresholdForDirection(const mixerProfileATDirection_e return 0; } +static bool shouldBlockManualDirectSwitchToFixedWing(const bool manualControllerEnabled, const int requestedProfileIndex) +{ + if (!manualControllerEnabled || !STATE(MULTIROTOR) || requestedProfileIndex == currentMixerProfileIndex) { + return false; + } + + if (mixerConfigByIndex(requestedProfileIndex)->platformType != PLATFORM_AIRPLANE) { + return false; + } + + const uint16_t thresholdCmS = getAirspeedThresholdForDirection(MIXERAT_DIRECTION_TO_FW); + if (thresholdCmS == 0 || !hasPitotSensorForManualProtection()) { + return false; + } + + float airspeedCmS = 0.0f; + if (!hasTrustedPitotAirspeed(&airspeedCmS)) { + return true; + } + + return airspeedCmS < thresholdCmS; +} + +static bool shouldRequestManualFwToMcProtection(const bool manualControllerEnabled) +{ + if (!manualControllerEnabled || !STATE(AIRPLANE)) { + return false; + } + + const uint16_t thresholdCmS = systemConfig()->vtolFwToMcAutoSwitchAirspeed; + if (thresholdCmS == 0 || !hasPitotSensorForManualProtection()) { + return false; + } + + float airspeedCmS = 0.0f; + if (!hasTrustedPitotAirspeed(&airspeedCmS)) { + return false; + } + + return airspeedCmS <= thresholdCmS; +} + static void updateTransitionScales(void) { if (!currentMixerConfig.vtolTransitionDynamicMixer) { @@ -477,6 +548,8 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) const bool missionActive = (navGetCurrentStateFlags() & NAV_AUTO_WP) != 0; const bool manualControllerConfigured = currentMixerConfig.manualVtolTransitionController && !missionActive; bool manualControllerEnabled = manualControllerConfigured || manualTransitionSessionLatched; + const bool mixerProfileModePresent = isModeActivationConditionPresent(BOXMIXERPROFILE); + const int requestedProfileIndex = IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1; if (manualControllerConfigured && transitionModeRisingEdge) { manualTransitionSessionLatched = true; @@ -496,14 +569,22 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) const bool suppressDirectProfileSwitch = manualControllerEnabled && transitionModeActive; if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse && !suppressDirectProfileSwitch) { - if (isModeActivationConditionPresent(BOXMIXERPROFILE)){ - outputProfileHotSwitch(IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1); + if (mixerProfileModePresent && + !shouldBlockManualDirectSwitchToFixedWing(manualControllerEnabled, requestedProfileIndex)) { + outputProfileHotSwitch(requestedProfileIndex); } } // Recompute after a potential direct profile hot-switch because this flag is per-mixer-profile. manualControllerEnabled = (currentMixerConfig.manualVtolTransitionController && !missionActive) || manualTransitionSessionLatched; + if (!mixerAT_inuse && + shouldRequestManualFwToMcProtection(manualControllerEnabled) && + checkMixerATRequired(MIXERAT_REQUEST_MANUAL_TO_MC)) { + mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_MC); + mixerAT_inuse = mixerATIsActive(); + } + if (!manualControllerEnabled) { // Backward-compatible manual path: level-controlled transition mixing request. if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) { diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 59fe4384809..13f690b5362 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,7 +18,6 @@ typedef struct mixerConfig_s { bool controlProfileLinking; bool automated_switch; int16_t switchTransitionTimer; - uint16_t switchTransitionAirspeed; bool vtolTransitionDynamicMixer; bool manualVtolTransitionController; uint16_t vtolTransitionAirspeedTimeoutMs; @@ -69,6 +68,7 @@ typedef struct mixerProfileAT_s { bool usedAirspeed; bool transitionStartAirspeedCaptured; float progress; + float scalingProgress; float transitionStartAirspeedCmS; float blendToFw; float pusherScale; From 061c7f23a20d2b3c05417cbfcb9f93f0736c5cdb Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Wed, 3 Jun 2026 19:35:59 +0300 Subject: [PATCH 19/42] vtol: decouple MC->FW pusher ramp from handoff scaling Use mixer_vtol_transition_scale_ramp_time_ms for MC->FW pusher ramp independently of airspeed progress, so the pusher can reach full authority without being limited by low initial airspeed. Keep lift, MC authority and FW authority scaling on the existing handoff path: airspeed-based when trusted pitot is available, with timer/progress fallback when it is not. Update VTOL and mixer profile docs to describe the split scaling behavior. --- docs/MixerProfile.md | 22 +++++++----- docs/Settings.md | 2 +- docs/VTOL.md | 28 +++++++++++----- src/main/fc/settings.yaml | 2 +- src/main/flight/mixer_profile.c | 59 +++++++++++++++++++++------------ src/main/flight/mixer_profile.h | 2 +- 6 files changed, 73 insertions(+), 42 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 69d543338dc..53dc7272fbc 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -117,9 +117,9 @@ When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Mod Manual `MIXER TRANSITION` and mission-authorized VTOL transition both use the same internal transition controller. This controller always computes transition progress/completion and performs its own profile hot-switch only inside the authorized transition state. Direct manual `MIXER PROFILE 2` switching remains a separate path when no transition controller path is active. -When `mixer_vtol_transition_dynamic_mixer = ON`, pusher/lift/authority scaling is enabled and is driven by: -- transition progress (default), or -- `mixer_vtol_transition_scale_ramp_time_ms` when configured (>0). +When `mixer_vtol_transition_dynamic_mixer = ON`, transition scaling is split into: +- a pusher ramp for MC->FW, and +- handoff scaling for lift / MC authority / FW authority. ### Airspeed-first completion @@ -152,9 +152,14 @@ With dynamic scaling enabled, `vtol_transition_fw_authority_start_percent = 100` Optional scaling ramp timer: -- trusted pitot available/healthy: scaling follows airspeed-based transition progress. -- `mixer_vtol_transition_scale_ramp_time_ms > 0`: if trusted pitot becomes unavailable/unhealthy, scaling falls back to this timer. -- `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): if trusted pitot is unavailable/unhealthy, scaling falls back to transition progress/timer behavior. +- MC->FW pusher: + - `mixer_vtol_transition_scale_ramp_time_ms > 0`: pusher ramps from `0 -> 100%` over this time, even when trusted pitot is healthy. + - `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): pusher goes to `100%` immediately. +- Lift / MC authority / FW authority handoff: + - trusted pitot available/healthy: follows airspeed-based transition progress. + - `mixer_vtol_transition_scale_ramp_time_ms > 0`: if trusted pitot becomes unavailable/unhealthy, handoff scaling falls back to this timer. + - `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): if trusted pitot is unavailable/unhealthy, handoff scaling falls back to transition progress/timer behavior. +- FW->MC keeps the existing handoff-based scaling behavior. Example: @@ -162,8 +167,9 @@ Example: - `mixer_vtol_transition_scale_ramp_time_ms = 1200` Result: -- when trusted pitot is healthy, scaling still follows airspeed progress, -- if trusted pitot becomes unavailable/unhealthy, scaling reaches target levels in ~1.2s, +- in MC->FW, pusher reaches full scale in ~1.2s, +- when trusted pitot is healthy, lift/MC/FW handoff still follows airspeed progress, +- if trusted pitot becomes unavailable/unhealthy, handoff scaling reaches target levels in ~1.2s, - transition completion still follows airspeed threshold when pitot is healthy, - timer fallback completion still uses 5s when pitot is unavailable/unhealthy. diff --git a/docs/Settings.md b/docs/Settings.md index d83167504f1..5a949bc2aee 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3244,7 +3244,7 @@ Enables dynamic VTOL transition progress/scaling controller shared by mission-au ### mixer_vtol_transition_scale_ramp_time_ms -Optional dynamic scaling fallback ramp duration [ms]. When > 0 and `mixer_vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling still follows trusted pitot-based transition progress when available; if trusted pitot becomes unavailable/unhealthy, scaling falls back to this timer. If set to 0, scaling falls back to transition progress/timer behavior. +Optional VTOL transition scaling ramp duration [ms]. In MC->FW, pusher scaling uses this timer regardless of pitot availability; if set to 0, pusher goes to full scale immediately. Lift/MC/FW handoff scaling still follows trusted pitot-based transition progress when available; if trusted pitot becomes unavailable/unhealthy, it falls back to this timer. If set to 0, handoff scaling falls back to transition progress/timer behavior. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md index 7221abf8400..fb2afc85dba 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -387,17 +387,23 @@ When `mixer_vtol_transition_dynamic_mixer = ON`, transition progress additionall When `mixer_vtol_transition_dynamic_mixer = OFF`, legacy static transition mixing behavior is preserved. Optional decoupled scaling ramp: -- trusted pitot available/healthy: scaling follows airspeed-based transition progress. -- `mixer_vtol_transition_scale_ramp_time_ms > 0`: if trusted pitot becomes unavailable/unhealthy, scaling falls back to this ramp timer. -- `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): if trusted pitot is unavailable/unhealthy, scaling falls back to transition progress/timer behavior. +- MC->FW pusher: + - `mixer_vtol_transition_scale_ramp_time_ms > 0`: pusher ramps from `0 -> 100%` over this time, even when trusted pitot is healthy. + - `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): pusher goes to `100%` immediately. +- Lift / MC authority / FW authority handoff: + - trusted pitot available/healthy: follows airspeed-based transition progress. + - `mixer_vtol_transition_scale_ramp_time_ms > 0`: if trusted pitot becomes unavailable/unhealthy, handoff scaling falls back to this ramp timer. + - `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): if trusted pitot is unavailable/unhealthy, handoff scaling falls back to transition progress/timer behavior. +- FW->MC keeps the existing handoff-based scaling behavior. Example: - `mixer_switch_trans_timer = 50` (5s fallback completion timer) - `mixer_vtol_transition_scale_ramp_time_ms = 1200` Result: -- when trusted pitot is healthy, pusher/lift/authority scaling still follows airspeed progress, -- if trusted pitot becomes unavailable/unhealthy, scaling reaches target levels in ~1.2s, +- in MC->FW, pusher reaches full scale in ~1.2s, +- when trusted pitot is healthy, lift/MC/FW handoff still follows airspeed progress, +- if trusted pitot becomes unavailable/unhealthy, handoff scaling reaches target levels in ~1.2s, - transition completion still follows airspeed thresholds when pitot is healthy, - if pitot is unavailable/unhealthy, completion fallback still uses 5s. @@ -448,7 +454,8 @@ CLI: What this does: - MC->FW completes primarily on pitot airspeed (1300 cm/s), with timer fallback only if pitot is unavailable/unhealthy. - FW->MC completes when airspeed drops to 850 cm/s. -- Scaling ramps quickly (1.2 s) to reduce step torque and abrupt authority handoff. +- In MC->FW, pusher ramps to full scale in 1.2 s while lift/MC/FW handoff still follows airspeed progress. +- The pusher ramp is quick enough (1.2 s) to reduce step torque while still allowing strong acceleration. - Timeout abort protects against staying too long in airspeed-controlled transition without reaching threshold. #### Test 3 - Mission-authorized transition (end-to-end mission flow) @@ -635,9 +642,12 @@ Dynamic mixer scaling (`mixer_vtol_transition_dynamic_mixer = ON`) uses this pro - MC authority ramps `vtol_transition_mc_authority_end_percent -> 1` - FW authority ramps `1 -> vtol_transition_fw_authority_start_percent` -Dynamic scaling prefers trusted pitot-based transition progress whenever available. -If trusted pitot becomes unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms > 0`, scaling falls back to that timer-based ramp. -If trusted pitot is unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms = 0`, scaling falls back to transition progress/timer behavior (`mixer_switch_trans_timer`). +Dynamic scaling splits MC->FW pusher ramp from lift/authority handoff scaling. +For MC->FW, pusher scaling uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, pusher goes full immediately. +Lift / MC authority / FW authority handoff still prefers trusted pitot-based transition progress whenever available. +If trusted pitot becomes unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms > 0`, handoff scaling falls back to that timer-based ramp. +If trusted pitot is unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms = 0`, handoff scaling falls back to transition progress/timer behavior (`mixer_switch_trans_timer`). +FW->MC keeps the existing handoff-based scaling behavior. For transition/pusher motors (`-2.0 < throttle < -1.0`), output is interpolated from idle to target: diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 75dd54e4ae4..c13536c5971 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1303,7 +1303,7 @@ groups: min: 0 max: 60000 - name: mixer_vtol_transition_scale_ramp_time_ms - description: "Optional dynamic scaling fallback ramp duration [ms]. When > 0 and `mixer_vtol_transition_dynamic_mixer` is ON, pusher/lift/authority scaling still follows trusted pitot-based transition progress when available; if trusted pitot becomes unavailable/unhealthy, scaling falls back to this timer. If set to 0, scaling falls back to transition progress/timer behavior." + description: "Optional VTOL transition scaling ramp duration [ms]. In MC->FW, pusher scaling uses this timer regardless of pitot availability; if set to 0, pusher goes to full scale immediately. Lift/MC/FW handoff scaling still follows trusted pitot-based transition progress when available; if trusted pitot becomes unavailable/unhealthy, it falls back to this timer. If set to 0, handoff scaling falls back to transition progress/timer behavior." default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs min: 0 diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index f305b57459b..31451dc0366 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -128,7 +128,7 @@ void setMixerProfileAT(void) mixerProfileAT.usedAirspeed = false; mixerProfileAT.transitionStartAirspeedCaptured = false; mixerProfileAT.progress = 0.0f; - mixerProfileAT.scalingProgress = 0.0f; + mixerProfileAT.handoffScalingProgress = 0.0f; mixerProfileAT.transitionStartAirspeedCmS = 0.0f; mixerProfileAT.blendToFw = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW ? 0.0f : 1.0f; mixerProfileAT.pusherScale = 1.0f; @@ -162,7 +162,7 @@ static mixerProfileATDirection_e directionForRequest(const mixerProfileATRequest static void resetTransitionScales(void) { mixerProfileAT.progress = 0.0f; - mixerProfileAT.scalingProgress = 0.0f; + mixerProfileAT.handoffScalingProgress = 0.0f; mixerProfileAT.blendToFw = 0.0f; mixerProfileAT.pusherScale = 0.0f; mixerProfileAT.liftScale = 1.0f; @@ -173,7 +173,7 @@ static void resetTransitionScales(void) static void setLegacyTransitionScales(void) { mixerProfileAT.progress = 1.0f; - mixerProfileAT.scalingProgress = 1.0f; + mixerProfileAT.handoffScalingProgress = 1.0f; mixerProfileAT.blendToFw = 1.0f; mixerProfileAT.pusherScale = 1.0f; mixerProfileAT.liftScale = 1.0f; @@ -186,30 +186,43 @@ static float blendScale(float from, float to, float progress) return from + (to - from) * constrainf(progress, 0.0f, 1.0f); } -static float getScalingProgress(void) +static float getPusherRampProgress(void) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer) { + return 1.0f; + } + + if (currentMixerConfig.vtolTransitionScaleRampTimeMs <= 0) { + return 1.0f; + } + + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + return constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); +} + +static float getHandoffScalingProgress(void) { if (!currentMixerConfig.vtolTransitionDynamicMixer) { return 1.0f; } if (mixerProfileAT.usedAirspeed) { - mixerProfileAT.scalingProgress = constrainf(mixerProfileAT.progress, 0.0f, 1.0f); - return mixerProfileAT.scalingProgress; + mixerProfileAT.handoffScalingProgress = constrainf(mixerProfileAT.progress, 0.0f, 1.0f); + return mixerProfileAT.handoffScalingProgress; } if (currentMixerConfig.vtolTransitionScaleRampTimeMs > 0) { - const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; - const float rampProgress = constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); + const float rampProgress = getPusherRampProgress(); - // Preserve already-applied scaling if pitot drops out mid-transition. - mixerProfileAT.scalingProgress = MAX(mixerProfileAT.scalingProgress, rampProgress); - return mixerProfileAT.scalingProgress; + // Preserve already-applied handoff scaling if pitot drops out mid-transition. + mixerProfileAT.handoffScalingProgress = MAX(mixerProfileAT.handoffScalingProgress, rampProgress); + return mixerProfileAT.handoffScalingProgress; } // Last-resort compatibility path: with no trusted pitot and no dedicated // scaling ramp configured, reuse transition progress/timer behavior. - mixerProfileAT.scalingProgress = MAX(mixerProfileAT.scalingProgress, constrainf(mixerProfileAT.progress, 0.0f, 1.0f)); - return mixerProfileAT.scalingProgress; + mixerProfileAT.handoffScalingProgress = MAX(mixerProfileAT.handoffScalingProgress, constrainf(mixerProfileAT.progress, 0.0f, 1.0f)); + return mixerProfileAT.handoffScalingProgress; } static bool hasTrustedPitotAirspeed(float *airspeedCmS) @@ -319,18 +332,20 @@ static void updateTransitionScales(void) const float liftFloor = constrainf(systemConfig()->vtolTransitionLiftEndPercent / 100.0f, 0.0f, 1.0f); const float mcFloor = constrainf(systemConfig()->vtolTransitionMcAuthorityEndPercent / 100.0f, 0.0f, 1.0f); const float fwFloor = constrainf(systemConfig()->vtolTransitionFwAuthorityStartPercent / 100.0f, 0.0f, 1.0f); - const float scaleProgress = getScalingProgress(); + const float handoffProgress = getHandoffScalingProgress(); if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { - mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, scaleProgress); - mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, scaleProgress); - mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, scaleProgress); - mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, scaleProgress); + const float pusherProgress = getPusherRampProgress(); + + mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, pusherProgress); + mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, handoffProgress); + mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, handoffProgress); + mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, handoffProgress); } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { - mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, scaleProgress); - mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, scaleProgress); - mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, scaleProgress); - mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, scaleProgress); + mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, handoffProgress); + mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, handoffProgress); + mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, handoffProgress); + mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, handoffProgress); } mixerProfileAT.blendToFw = constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 13f690b5362..cbc69066ea0 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -68,7 +68,7 @@ typedef struct mixerProfileAT_s { bool usedAirspeed; bool transitionStartAirspeedCaptured; float progress; - float scalingProgress; + float handoffScalingProgress; float transitionStartAirspeedCmS; float blendToFw; float pusherScale; From 94a6b345a8fc4956db0e3df28d253727705f5a32 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 4 Jun 2026 15:27:25 +0300 Subject: [PATCH 20/42] vtol: restore manual profile hot-switch during transition --- docs/MixerProfile.md | 1 - docs/VTOL.md | 1 - src/main/flight/mixer_profile.c | 30 ++---------------------------- 3 files changed, 2 insertions(+), 30 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 53dc7272fbc..b0609100772 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -35,7 +35,6 @@ The use of Transition Mode is recommended to enable further features and future - Keeping the mode ON does not repeatedly restart transitions. - A new transition requires mode OFF then ON again. - If switched OFF before hot-switch completes, the manual transition request is aborted. -- If valid pitot is present and MC->FW threshold is configured, direct manual profile hot-switch to FW is blocked until threshold is reached. - Optional FW protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can auto-request FW->MC transition when valid pitot airspeed drops to/below the configured value (`0` disables). This edge-triggered behavior is enabled by `mixer_vtol_manualswitch_autotransition_controller`. diff --git a/docs/VTOL.md b/docs/VTOL.md index fb2afc85dba..33275f5a899 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -320,7 +320,6 @@ With `mixer_vtol_manualswitch_autotransition_controller = ON`: - Keeping the mode ON does not repeatedly retrigger transition. - To start another transition, mode must go OFF then ON again. - If mode is turned OFF before hot-switch, transition request is aborted safely. -- If valid pitot is present and MC->FW airspeed threshold is configured, direct manual profile hot-switch to FW is blocked until threshold is reached. - Optional FW safety fallback: set `vtol_fw_to_mc_auto_switch_airspeed_cm_s > 0` to auto-request FW->MC transition when valid pitot airspeed drops to/below the configured value. With `mixer_vtol_manualswitch_autotransition_controller = OFF`: diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 31451dc0366..7f477b9b6a4 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -276,29 +276,6 @@ static uint16_t getAirspeedThresholdForDirection(const mixerProfileATDirection_e return 0; } -static bool shouldBlockManualDirectSwitchToFixedWing(const bool manualControllerEnabled, const int requestedProfileIndex) -{ - if (!manualControllerEnabled || !STATE(MULTIROTOR) || requestedProfileIndex == currentMixerProfileIndex) { - return false; - } - - if (mixerConfigByIndex(requestedProfileIndex)->platformType != PLATFORM_AIRPLANE) { - return false; - } - - const uint16_t thresholdCmS = getAirspeedThresholdForDirection(MIXERAT_DIRECTION_TO_FW); - if (thresholdCmS == 0 || !hasPitotSensorForManualProtection()) { - return false; - } - - float airspeedCmS = 0.0f; - if (!hasTrustedPitotAirspeed(&airspeedCmS)) { - return true; - } - - return airspeedCmS < thresholdCmS; -} - static bool shouldRequestManualFwToMcProtection(const bool manualControllerEnabled) { if (!manualControllerEnabled || !STATE(AIRPLANE)) { @@ -580,12 +557,9 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) mixerAT_inuse = false; } - // For manual auto-transition control, suppress direct profile hotswitch while transition trigger is active. - const bool suppressDirectProfileSwitch = manualControllerEnabled && transitionModeActive; - if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse && !suppressDirectProfileSwitch) + if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) { - if (mixerProfileModePresent && - !shouldBlockManualDirectSwitchToFixedWing(manualControllerEnabled, requestedProfileIndex)) { + if (mixerProfileModePresent) { outputProfileHotSwitch(requestedProfileIndex); } } From 02e7f86f5b9f916c461a9d96ff29e5a7ffd64ed6 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 4 Jun 2026 19:13:19 +0300 Subject: [PATCH 21/42] vtol: simplify transition docs and remove mission track-distance setting Rewrite VTOL docs and setting descriptions in simpler user-facing language, including clearer explanations for transition behavior, mission USER flags, and smooth mixer handover. Remove nav_vtol_mission_transition_track_distance_cm from user-facing configuration and use a fixed internal MC->FW mission run-up distance instead. Bump PG_NAV_CONFIG after removing the stored nav config field. --- docs/MixerProfile.md | 187 +++++++++++++------------ docs/Navigation.md | 2 +- docs/Settings.md | 44 +++--- docs/VTOL.md | 232 +++++++++++++++---------------- src/main/fc/settings.yaml | 38 +++-- src/main/navigation/navigation.c | 7 +- src/main/navigation/navigation.h | 1 - 7 files changed, 251 insertions(+), 260 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index b0609100772..c966cf8c501 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -22,44 +22,50 @@ Please note that this is an emerging / experimental capability that will require ## Mixer Transition input -Typically, 'transition input' will be useful in MR mode to gain airspeed. -The associated motor or servo will then move accordingly when transition mode is activated. -Transition input is disabled when navigation mode is activate +`MIXER TRANSITION` is mainly used while the model is still in multicopter mode, so a forward motor or tilt servo can help the aircraft build forward speed before the switch to fixed-wing. -The use of Transition Mode is recommended to enable further features and future developments like fail-safe support. Mapping motor to servo output, or servo with logic conditions is **not** recommended +This feature is recommended for VTOL setups. It is normally blocked while navigation modes are active. +Mapping a motor to a servo output, or using servo logic conditions for this feature, is **not** recommended. -`MIXER TRANSITION` now behaves as a transition trigger/request (edge-triggered), not a continuous blend hold: +If `mixer_vtol_manualswitch_autotransition_controller = ON`, `MIXER TRANSITION` works like a start switch for one transition: -- A rising edge starts one transition (MC->FW or FW->MC depending on current profile). -- The transition state machine runs automatically to completion. -- Keeping the mode ON does not repeatedly restart transitions. -- A new transition requires mode OFF then ON again. -- If switched OFF before hot-switch completes, the manual transition request is aborted. -- Optional FW protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can auto-request FW->MC transition when valid pitot airspeed drops to/below the configured value (`0` disables). +- Each time you move `MIXER TRANSITION` from OFF to ON, iNAV starts one transition. +- The same switch can be used in both directions: + - MC -> transition -> FW + - FW -> transition -> MC +- After it starts, the transition keeps running until the speed target or timer target is reached. +- Leaving the switch ON does not keep restarting the transition. +- To start another transition, turn the switch OFF and then ON again. +- If you turn the switch OFF before the profile change happens, that transition request is cancelled. +- Optional extra protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can automatically start FW->MC if airspeed gets too low. -This edge-triggered behavior is enabled by `mixer_vtol_manualswitch_autotransition_controller`. -Set `mixer_vtol_manualswitch_autotransition_controller = ON` in both mixer profiles (MC and FW) used for switching to keep manual transition semantics consistent after profile hot-switch. -When `mixer_vtol_manualswitch_autotransition_controller = OFF`, manual transition keeps legacy behavior. -With manual auto-transition enabled, Active Modes `MIXER TRANSITION` now indicates that the internal transition controller/mixing is actually active, not merely that the RC `MIXER TRANSITION` switch is active. -Active Modes `MIXER PROFILE 2` indicates the currently active mixer profile. +This behavior is controlled by `mixer_vtol_manualswitch_autotransition_controller`. +Turn it ON in both mixer profiles if you want the same switch behavior in both directions. +If it is OFF, manual transition keeps the older behavior. -Important path split: -- `MIXER PROFILE 2` remains a direct manual profile-switch path. -- Smooth VTOL transition state-machine behavior is triggered by `MIXER TRANSITION` when `mixer_vtol_manualswitch_autotransition_controller = ON`. +In Active Modes: + +- `MIXER TRANSITION` shows that the internal transition logic is actually running. +- `MIXER PROFILE 2` shows that mixer profile 2 is currently active. + +There are two separate manual paths: + +- `MIXER PROFILE 2` is still a direct manual profile switch. +- `MIXER TRANSITION` starts the smooth automatic transition sequence when `mixer_vtol_manualswitch_autotransition_controller = ON`. + +Recommended 3-position switch example: -Recommended switch topology example (clear 3-position setup): - This example assumes the usual VTOL order used in this document: - Profile 1 = FW - Profile 2 = MC - Use a dedicated 3-position mapping: - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) - - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) + - Pos2 = Transition request (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) - Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. -- Avoid overlapping FW selection and transition trigger in the same position. -- Avoid 2-position setups where one position activates both `MIXER PROFILE 2` and `MIXER TRANSITION`. -- Overlapping mode activation can produce order-dependent behavior (direct profile switch path vs transition-controller path), which is unpredictable and not recommended. -- If you intentionally swap the profile order, the same idea still works; just swap the FW and MC end positions. +- Avoid a switch position that turns ON both `MIXER PROFILE 2` and `MIXER TRANSITION`. +- If both are ON together, iNAV may switch profile immediately instead of running the smooth transition. +- If you intentionally swap the profile order, keep the same idea and swap the FW and MC end positions. ## Servo @@ -95,7 +101,10 @@ It is recommend that the pilot uses a RC mode switch to activate modes or switch Profile files Switching is not available until the runtime sensor calibration is done. Switching is NOT available when navigation mode is activate. `mixer_profile` 1 will be used as default, `mixer_profile` 2 will be used when the `MIXER PROFILE 2` mode box is activated. -Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. Here is sample of using these RC modes: +Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. + +The example below is a **legacy manual switch** example, where `MIXER TRANSITION` is used as a live transition input and `mixer_vtol_manualswitch_autotransition_controller = OFF`. +It is not the recommended mapping for the newer automatic transition controller. ![Alt text](Screenshots/mixer_profile.png) @@ -105,60 +114,67 @@ Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input It is also possible to set it as 4 state switch by adding FW(profile1) with transition on. +If `mixer_vtol_manualswitch_autotransition_controller = ON`, do **not** use this overlap style where `MIXER PROFILE 2` and `MIXER TRANSITION` are ON together. +For the newer smooth automatic transition behavior, use the dedicated 3-position mapping shown earlier in this document. + ## Automated Transition -This feature is mainly for RTH in a failsafe event. When set properly, model will use the FW mode to fly home efficiently, And land in the MC mode for easier landing. -`ON` for a mixer_profile\`s `mixer_automated_switch` means to schedule a Automated Transition when RTH head home(applies for MC mixer_profile) or RTH Land(applies for FW mixer_profile) is requested by navigation controller. -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. +This feature is mainly used for RTH and failsafe. +When set up correctly, the aircraft can use fixed-wing for the efficient flight home and then return to multicopter for easier landing. + +If `mixer_automated_switch = ON` in a mixer profile, iNAV is allowed to run an automated transition when the navigation logic asks for it. + +- Use `mixer_automated_switch = ON` in the MC mixer profile if you want automated MC->FW transition during the head-home part of RTH. +- Use `mixer_automated_switch = ON` in the FW mixer profile if you want automated FW->MC transition for the landing part. +- Set `mixer_switch_trans_timer` in each profile to a sensible backup time for that direction. + +If `mixer_automated_switch = OFF` in all mixer profiles, automated VTOL transition is disabled. ### Unified VTOL transition controller -Manual `MIXER TRANSITION` and mission-authorized VTOL transition both use the same internal transition controller. -This controller always computes transition progress/completion and performs its own profile hot-switch only inside the authorized transition state. -Direct manual `MIXER PROFILE 2` switching remains a separate path when no transition controller path is active. -When `mixer_vtol_transition_dynamic_mixer = ON`, transition scaling is split into: -- a pusher ramp for MC->FW, and -- handoff scaling for lift / MC authority / FW authority. +Manual `MIXER TRANSITION` and mission-requested VTOL transition both use the same internal transition controller. +That means the same airspeed checks, timer fallback, and smooth power changes are reused in both cases. + +Direct manual `MIXER PROFILE 2` switching is still a separate path when you want an immediate profile change. ### Airspeed-first completion -When pitot airspeed is healthy and available, transition completion uses pitot thresholds: +When valid pitot airspeed is available, iNAV uses airspeed to decide when the transition is complete: - `vtol_transition_to_fw_min_airspeed_cm_s` for MC->FW - `vtol_transition_to_mc_max_airspeed_cm_s` for FW->MC -If pitot is unavailable/unhealthy (or threshold is `0`), timer fallback is used (`mixer_switch_trans_timer`). +If pitot is not available, not healthy, or the threshold is set to `0`, iNAV uses `mixer_switch_trans_timer` instead. Ground speed is not used for transition completion/progress. -Optional safety timeout: +Optional timeout: -- `mixer_vtol_transition_airspeed_timeout_ms` can abort transition if airspeed condition is not met in time. -- This timeout is only active while transition completion is using trusted pitot airspeed. -- If pitot is unavailable/unhealthy, transition completion falls back to `mixer_switch_trans_timer`; timeout does not force abort in that fallback path. -- For airspeed-first setups, configure non-zero `mixer_switch_trans_timer` fallback (typical `40..60`, i.e. `4..6s`) so pitot-loss fallback does not complete immediately. +- `mixer_vtol_transition_airspeed_timeout_ms` limits how long iNAV waits for the required airspeed. +- This timeout is only used while pitot airspeed is actually controlling the transition. +- If pitot is lost, iNAV falls back to `mixer_switch_trans_timer` and this timeout no longer decides the outcome. +- For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). -### Dynamic scaling (optional) +### Smooth power changes during transition (optional) -When `mixer_vtol_transition_dynamic_mixer = ON`, transition progress scales: +When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: -- pusher contribution (`-2.0 < throttle < -1.0` motors) from configured max toward 0/100% depending on direction, -- lift motor throttle contribution (`vtol_transition_lift_end_percent`), -- MC stabilization authority (`vtol_transition_mc_authority_end_percent`), -- FW authority start level (`vtol_transition_fw_authority_start_percent`, servo transition input blend). +- forward motor power, +- lift motor power, +- multicopter stabilisation strength, +- fixed-wing control strength. Default is OFF to preserve existing behavior. -With dynamic scaling enabled, `vtol_transition_fw_authority_start_percent = 100` preserves legacy FW authority handoff; lower values provide smoother ramp-in. +With it ON, `vtol_transition_fw_authority_start_percent = 100` keeps the old fixed-wing control behavior. Lower values bring fixed-wing control in more gently. -Optional scaling ramp timer: +How `mixer_vtol_transition_scale_ramp_time_ms` works: - MC->FW pusher: - - `mixer_vtol_transition_scale_ramp_time_ms > 0`: pusher ramps from `0 -> 100%` over this time, even when trusted pitot is healthy. - - `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): pusher goes to `100%` immediately. -- Lift / MC authority / FW authority handoff: - - trusted pitot available/healthy: follows airspeed-based transition progress. - - `mixer_vtol_transition_scale_ramp_time_ms > 0`: if trusted pitot becomes unavailable/unhealthy, handoff scaling falls back to this timer. - - `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): if trusted pitot is unavailable/unhealthy, handoff scaling falls back to transition progress/timer behavior. -- FW->MC keeps the existing handoff-based scaling behavior. + - `> 0`: forward motor power ramps from `0 -> 100%` over this time, even when pitot is working normally. + - `= 0` (default): forward motor power goes to `100%` immediately. +- Lift motor power, MC stabilisation, and FW control: + - with valid pitot airspeed, they still follow transition progress based on airspeed. + - if pitot stops being usable and this setting is `> 0`, they use this same timer as a backup ramp. + - if pitot stops being usable and this setting is `0`, they fall back to the normal transition timer/progress behavior. +- FW->MC keeps the existing style of smooth handover. Example: @@ -166,21 +182,22 @@ Example: - `mixer_vtol_transition_scale_ramp_time_ms = 1200` Result: -- in MC->FW, pusher reaches full scale in ~1.2s, -- when trusted pitot is healthy, lift/MC/FW handoff still follows airspeed progress, -- if trusted pitot becomes unavailable/unhealthy, handoff scaling reaches target levels in ~1.2s, -- transition completion still follows airspeed threshold when pitot is healthy, -- timer fallback completion still uses 5s when pitot is unavailable/unhealthy. +- in MC->FW, the forward motor reaches full power in about `1.2s`, +- when pitot is working, lift power and control handover still follow airspeed, +- if pitot stops being usable, the same handover reaches its target in about `1.2s`, +- transition completion still uses airspeed when pitot is working, +- backup completion time is still `5s` if pitot is not usable. ### Mission-authorized VTOL transition (waypoint User Action) -INAV supports mission-requested VTOL transitions through the existing automated transition path. This is configured with: +INAV can also change between MC and FW from the mission itself. +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) - `vtol_transition_to_fw_min_airspeed_cm_s` (preferred MC->FW threshold) -Scope note: +Setting scope: - Per-mixer-profile settings: - `mixer_automated_switch` @@ -198,24 +215,21 @@ Scope note: - `vtol_transition_fw_authority_start_percent` - `nav_vtol_mission_transition_user_action` - `nav_vtol_mission_transition_min_altitude_cm` - - `nav_vtol_mission_transition_track_distance_cm` -On each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`), the configured USER action bit is used as absolute target selector: +On each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`), the chosen USER flag tells iNAV which flight mode should be active there: -- selected USER bit = `0` -> transition to MC / MULTIROTOR profile -- selected USER bit = `1` -> transition to FW / AIRPLANE profile -- When `nav_vtol_mission_transition_user_action != OFF`, each navigable waypoint encodes a target state via that selected bit. -- This is a per-waypoint target-state declaration (not an event trigger). Users should intentionally set/clear the selected USER bit on each navigable waypoint. -- This is **not** a toggle command. -- If already in the requested profile type, the action is treated as complete (idempotent). +- selected USER flag = `0` -> target MC / MULTIROTOR profile +- selected USER flag = `1` -> target FW / AIRPLANE profile +- This is set per waypoint. It is **not** a toggle command. +- If the aircraft is already in the requested mode, iNAV does nothing and continues. 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. -Mission path uses the same controller and completion logic as manual transition (airspeed-first, timer fallback). +For MC->FW mission transitions, navigation uses a built-in straight acceleration run to build speed before the switch to fixed-wing. +Mission transition uses the same transition logic as manual transition: airspeed first, timer as backup. -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. -Mission VTOL transition still relies on normal profile-switch infrastructure: configure two mixer profiles and a valid `MIXER PROFILE 2` mode activation condition. +Manual RC switching (`MIXER PROFILE 2`, `MIXER TRANSITION`) is still blocked during normal active navigation. +Mission VTOL transition still relies on the normal two-profile setup, so you must configure both mixer profiles and a valid `MIXER PROFILE 2` mode condition. ### Example test presets (VTOL ~1.0m wingspan, ~1720g AUW) @@ -234,10 +248,10 @@ CLI: - `set nav_vtol_mission_transition_user_action = OFF` Behavior: -- Preserves legacy-style transition mixing while still using the new controller path. -- Useful as a known-safe baseline before enabling dynamic scaling. +- Keeps behavior close to the older transition setup. +- Good as a known-safe starting point before enabling the smoother power changes. -#### Test 2 - Airspeed-first + dynamic scaling (manual tuning) +#### Test 2 - Airspeed-first + smooth power changes (manual tuning) CLI: - `set mixer_vtol_manualswitch_autotransition_controller = ON` @@ -253,8 +267,8 @@ CLI: - `set nav_vtol_mission_transition_user_action = OFF` Behavior: -- Uses pitot-first completion logic with timer fallback only when pitot is unavailable/unhealthy. -- Adds fast, smooth pusher/lift/authority ramping to reduce abrupt transitions. +- Uses pitot airspeed first, with timer fallback only if pitot is not usable. +- Adds smoother forward-motor, lift-motor, and control handover to reduce abrupt transitions. #### Test 3 - Mission-authorized transition (mission integration) @@ -271,10 +285,9 @@ CLI: - `set vtol_transition_fw_authority_start_percent = 20` - `set nav_vtol_mission_transition_user_action = USER1` - `set nav_vtol_mission_transition_min_altitude_cm = 1200` -- `set nav_vtol_mission_transition_track_distance_cm = 4000` Behavior: -- Uses USER1 as per-waypoint absolute target selector (clear=MC, set=FW). +- Uses USER1 as the per-waypoint target selector (`clear = MC`, `set = FW`). - Pauses mission progression during transition and resumes only after transition completion. ### Validation Matrix (PR / SITL / HITL) @@ -284,7 +297,7 @@ Behavior: - FW->MC manual, pitot healthy/available. - FW->MC manual, no pitot (timer fallback). - `MIXER TRANSITION` held ON after completion (no repeated starts). -- `MIXER TRANSITION` OFF before hot-switch (safe abort). +- `MIXER TRANSITION` OFF before profile change (safe abort). - Mission transition with selected USER bit = `1` (TO_FW). - Mission transition with selected USER bit = `0` (TO_MC). - Failsafe/disarm during active transition (abort and no blind mission resume). @@ -306,7 +319,7 @@ Debug channels: - bit3: transition mixing output active (`isMixerTransitionMixing`) - bit4: RC `MIXERTRANSITION` mode active - bit5: airspeed-controlled path in use - - bit6: hot-switch done + - bit6: profile change done - bit7: transition aborted - bit8: manual VTOL auto-transition controller enabled in current mixer config - bit9: dynamic transition mixer enabled in current mixer config @@ -321,7 +334,7 @@ Debug channels: - `debug[3]` = progress x1000 (`0..1000`) - `debug[4]` = pusher scale x1000 (`0..1000`) - `debug[5]` = lift scale x1000 (`0..1000`) -- `debug[6]` = MC authority scale x1000 (`0..1000`) +- `debug[6]` = MC stabilisation scale x1000 (`0..1000`) - `debug[7]` = current mixer profile pitch transition PID multiplier (`transition_PID_mmix_multiplier_pitch`) ## TailSitter (planned for INAV 7.1) diff --git a/docs/Navigation.md b/docs/Navigation.md index 1a96d947d40..e3598cf5fc0 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -110,7 +110,7 @@ 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. +- During MC->FW mission transition, iNAV uses a built-in straight run-up target to help the model build speed before switching to fixed-wing. - VTOL transition completion logic is shared with manual MIXER TRANSITION and uses mixer transition settings: - preferred MC->FW threshold: `vtol_transition_to_fw_min_airspeed_cm_s` - FW->MC threshold: `vtol_transition_to_mc_max_airspeed_cm_s` diff --git a/docs/Settings.md b/docs/Settings.md index 5a949bc2aee..14062ec4a33 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3202,9 +3202,7 @@ If enabled, control_profile_index will follow mixer_profile index. Set to OFF(de ### mixer_switch_trans_timer -If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. - -If trusted pitot is unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms = 0`, dynamic scaling also falls back to this transition progress/timer behavior. +Time, in deciseconds (0.1s), that transition motors or servos stay active before iNAV changes to the other mixer profile during an automated VTOL switch. This is also the backup transition time when pitot airspeed is not available. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, the other smooth transition power changes also fall back to this timing. | Default | Min | Max | | --- | --- | --- | @@ -3214,7 +3212,7 @@ If trusted pitot is unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_ ### mixer_vtol_manualswitch_autotransition_controller -Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior. For consistent manual transition semantics, enable this in both mixer profiles. +Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switch moves from OFF to ON, when not in waypoint mission. Turn this ON in both mixer profiles if you want the same behavior in both directions. OFF keeps the older manual switch behavior. | Default | Min | Max | | --- | --- | --- | @@ -3224,7 +3222,7 @@ Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` ### mixer_vtol_transition_airspeed_timeout_ms -Safety timeout [ms] for airspeed-controlled transitions. If non-zero and required airspeed condition is not met in time, transition aborts instead of force-completing. +How long iNAV will wait for the required pitot airspeed during an airspeed-based transition. If the target airspeed is not reached in time, the transition is aborted. Set to 0 to disable. | Default | Min | Max | | --- | --- | --- | @@ -3234,7 +3232,7 @@ Safety timeout [ms] for airspeed-controlled transitions. If non-zero and require ### mixer_vtol_transition_dynamic_mixer -Enables dynamic VTOL transition progress/scaling controller shared by mission-authorized and manual MIXER TRANSITION paths. +Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. | Default | Min | Max | | --- | --- | --- | @@ -3244,7 +3242,7 @@ Enables dynamic VTOL transition progress/scaling controller shared by mission-au ### mixer_vtol_transition_scale_ramp_time_ms -Optional VTOL transition scaling ramp duration [ms]. In MC->FW, pusher scaling uses this timer regardless of pitot availability; if set to 0, pusher goes to full scale immediately. Lift/MC/FW handoff scaling still follows trusted pitot-based transition progress when available; if trusted pitot becomes unavailable/unhealthy, it falls back to this timer. If set to 0, handoff scaling falls back to transition progress/timer behavior. +Ramp-up time [ms] for the forward motor during MC->FW when smooth VTOL transition power changes are ON. `0` gives full forward-motor power immediately. The same timer is also used as a backup for lift motor power, multicopter stabilisation, and fixed-wing control fade if pitot airspeed is lost or unavailable. | Default | Min | Max | | --- | --- | --- | @@ -4671,7 +4669,7 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT ### nav_vtol_mission_transition_min_altitude_cm -Minimum altitude [cm] required to start a mission-authorized VTOL transition. Set to 0 to disable the minimum-altitude check. +Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the altitude check. | Default | Min | Max | | --- | --- | --- | @@ -4679,19 +4677,9 @@ Minimum altitude [cm] required to start a mission-authorized VTOL transition. Se --- -### nav_vtol_mission_transition_track_distance_cm - -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 | Min | Max | -| --- | --- | --- | -| 100000 | 1000 | 500000 | - ---- - ### nav_vtol_mission_transition_user_action -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. This is an absolute per-waypoint target-state selector and relies on existing mixer profile switching infrastructure (two profiles and valid MIXER PROFILE 2 mode activation condition). +Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles and a working `MIXER PROFILE 2` mode setup. | Allowed Values | | | --- | --- | @@ -4705,7 +4693,7 @@ Selects which waypoint USER action bit (`USER1`..`USER4`) is used as mission VTO ### nav_vtol_transition_fail_action_fw_to_mc -Action executed after a final FW->MC transition failure. LOITER switches to POSHOLD hold at current position (fixed-wing loiter/orbit around current point). FORCE_SWITCH attempts an immediate mixer hot-switch even after failed criteria. +What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. | Allowed Values | | | --- | --- | @@ -4719,7 +4707,7 @@ Action executed after a final FW->MC transition failure. LOITER switches to POSH ### nav_vtol_transition_fail_action_mc_to_fw -Action executed after a final MC->FW transition failure (after retry logic, if enabled). +What iNAV should do if MC->FW transition still fails after the final attempt. | Allowed Values | | | --- | --- | @@ -4732,7 +4720,7 @@ Action executed after a final MC->FW transition failure (after retry logic, if e ### nav_vtol_transition_retry_on_airspeed_timeout -If ON, allows one retry for failed airspeed-gated MC->FW auto-transition (mission or RTH head-home): hold position, perform a 360deg yaw scan, align to best measured pitot airspeed heading, and retry transition once. +If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. | Default | Min | Max | | --- | --- | --- | @@ -7090,7 +7078,7 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, ### vtol_fw_to_mc_auto_switch_airspeed_cm_s -Automatic FW->MC protection threshold [cm/s] used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. If set above 0 and valid pitot airspeed is at/below this value while in FW, controller requests FW->MC transition automatically. Set to 0 to disable. +Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV can start FW->MC transition automatically. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. | Default | Min | Max | | --- | --- | --- | @@ -7100,7 +7088,7 @@ Automatic FW->MC protection threshold [cm/s] used only when `mixer_vtol_manualsw ### vtol_transition_fw_authority_start_percent -Initial fixed-wing authority scale at transition start, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. +How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring in fixed-wing control more gently. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. | Default | Min | Max | | --- | --- | --- | @@ -7110,7 +7098,7 @@ Initial fixed-wing authority scale at transition start, in percent. Used only wh ### vtol_transition_lift_end_percent -Target vertical-lift throttle scale at transition end, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. +How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. | Default | Min | Max | | --- | --- | --- | @@ -7120,7 +7108,7 @@ Target vertical-lift throttle scale at transition end, in percent. Used only whe ### vtol_transition_mc_authority_end_percent -Target multicopter stabilization authority scale at transition end, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. +How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. | Default | Min | Max | | --- | --- | --- | @@ -7130,7 +7118,7 @@ Target multicopter stabilization authority scale at transition end, in percent. ### vtol_transition_to_fw_min_airspeed_cm_s -Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. If 0, MC->FW uses timer fallback (`mixer_switch_trans_timer`). +Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete. If set to 0, iNAV uses the transition timer instead. | Default | Min | Max | | --- | --- | --- | @@ -7140,7 +7128,7 @@ Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspe ### vtol_transition_to_mc_max_airspeed_cm_s -Maximum pitot airspeed [cm/s] allowed to complete FW->MC transition when airspeed is healthy and available. If 0, FW->MC uses timer fallback. +When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower. If set to 0, iNAV uses the transition timer instead. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md index 33275f5a899..484ce28c47b 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -180,6 +180,11 @@ You must also assign the tilting servos values using the MAX values. If you don | :-- | :-- | :-- | | Profile1(FW) with transition off | Profile2(MC) with transition on | Profile2(MC) with transition off | +- This is a **legacy manual switch** example, where `MIXER TRANSITION` is used as a live transition input. +- It is suitable for the older manual behavior with `mixer_vtol_manualswitch_autotransition_controller = OFF`. +- If you use `mixer_vtol_manualswitch_autotransition_controller = ON`, do **not** use this overlap mapping. +- For the newer smooth automatic transition behavior, use the dedicated 3-position mapping described later in this document, where `MIXER PROFILE 2` and `MIXER TRANSITION` are not ON together. + - Profile file switching becomes available after completing the runtime sensor calibration (15-30s after booting). And It is **not available** when a navigation mode or position hold is active. - By default, `mixer_profile 1` is used. `mixer_profile 2` is used when the `MIXER PROFILE 2` mode is activate. Once configured successfully, you will notice that the profiles and model preview changes accordingly when you refresh the relevant INAV Configurator tabs. @@ -295,116 +300,114 @@ INAV now uses one internal VTOL transition controller for both: - manual `MIXER TRANSITION` requests, and - mission-authorized VTOL transitions. -This keeps one safety boundary for profile hot-switching and avoids separate transition implementations. +This keeps one safety boundary for profile changes and avoids separate transition implementations. ### Behavior summary -- Transition progress is always computed internally. -- Pitot airspeed is the primary source for transition completion when healthy/available. -- Timer is used as fallback when pitot is unavailable/unhealthy. +- iNAV always tracks transition progress internally. +- If valid pitot airspeed is available, airspeed is the main way iNAV decides when transition is complete. +- If pitot is not available, iNAV falls back to a timer. - Ground speed is not used for transition completion. -- Mission transition uses the same controller and does not directly manipulate motors. -- Manual `MIXER PROFILE` / `MIXER TRANSITION` bypass during normal waypoint navigation is still blocked. -- `MIXER PROFILE 2` remains a direct profile-switch path when used manually. -- Smooth/automatic transition behavior is triggered by `MIXER TRANSITION` (with manual auto-controller ON) or by mission-authorized transition requests. +- Mission VTOL transition uses the same controller and does not directly drive the motors by itself. +- During normal waypoint navigation, manual `MIXER PROFILE` and `MIXER TRANSITION` switching is still blocked. +- `MIXER PROFILE 2` is still a direct manual profile switch when you are flying manually. +- Smooth automatic transition is started by `MIXER TRANSITION` when the manual auto-controller is ON, or by a mission transition request. ### Manual transition semantics -Intent: this does not replace legacy manual behavior. Legacy remains available and selectable. +This does not remove the older manual behavior. The older behavior is still available if you want it. With `mixer_vtol_manualswitch_autotransition_controller = ON`: -- Enable this setting in both mixer profiles (MC and FW) for consistent edge-triggered behavior across profile hot-switches. -- `MIXER TRANSITION` acts as an edge-triggered request. -- A rising edge starts one transition. -- Transition then runs autonomously to completion. -- Keeping the mode ON does not repeatedly retrigger transition. -- To start another transition, mode must go OFF then ON again. -- If mode is turned OFF before hot-switch, transition request is aborted safely. -- Optional FW safety fallback: set `vtol_fw_to_mc_auto_switch_airspeed_cm_s > 0` to auto-request FW->MC transition when valid pitot airspeed drops to/below the configured value. +- Turn this ON in both mixer profiles if you want the same behavior in both directions. +- Each time `MIXER TRANSITION` moves from OFF to ON, iNAV starts one transition. +- After it starts, the transition keeps running until the speed target or timer target is reached. +- Leaving the switch ON does not keep restarting the transition. +- To start another transition, turn the switch OFF and then ON again. +- If you turn the switch OFF before the profile change happens, that transition request is cancelled. +- Optional extra protection: set `vtol_fw_to_mc_auto_switch_airspeed_cm_s > 0` if you want FW->MC to start automatically when pitot airspeed becomes too low. With `mixer_vtol_manualswitch_autotransition_controller = OFF`: -- legacy manual behavior is preserved for backward compatibility. +- the older manual behavior is preserved. -Typical 3-position switch workflow (edge-trigger mode enabled): -- Position 1: MC -- Position 2: Transition (trigger AUTO transition sequence) -- Position 3: FW +Typical 3-position switch workflow: +- Position 1: FW +- Position 2: Transition request +- Position 3: MC Operational example: -- fly in MC (pos1) -> move to Transition (pos2) to start automatic MC->FW transition -> after completion move to FW (pos3), -- reverse order for FW->MC. +- fly in MC (pos3) -> move to Transition (pos2) to start automatic MC->FW transition -> after completion move to FW (pos1) +- reverse the order for FW->MC Important RC mapping constraint: - Use a dedicated 3-position mapping where: - - Pos1 = MC (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) + - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) - - Pos3 = FW (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) + - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) - Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. -- Do not overlap/merge FW selection and transition trigger in the same switch position. -- Do not use a 2-position mapping where one position enables both `MIXER PROFILE 2` and `MIXER TRANSITION`. -- Mixing these mode conditions can cause race/order-dependent behavior (direct profile switch versus transition state machine), which is unpredictable in flight. +- Avoid a switch position that turns ON both `MIXER PROFILE 2` and `MIXER TRANSITION`. +- If both are ON together, iNAV may switch profile immediately instead of running the smooth transition. ### Mission-authorized transition semantics Mission transition is configured with `nav_vtol_mission_transition_user_action`. - `OFF`: feature disabled. -- `USER1`..`USER4`: selected User Action bit is used as target selector on navigable waypoints. -- selected bit `0` -> target MC profile -- selected bit `1` -> target FW profile -- when enabled, every navigable waypoint implicitly declares desired VTOL platform state through that selected bit, so users should set/clear it intentionally per waypoint +- `USER1`..`USER4`: the selected USER flag becomes the flight-mode selector on navigable waypoints. +- selected flag `0` -> target MC profile +- selected flag `1` -> target FW profile +- when this feature is ON, every navigable waypoint should intentionally have that USER flag either clear or set - Mission progression pauses during transition and resumes only after completion. -- If already in requested target profile, command is idempotent (no new transition). +- If the aircraft is already in the requested mode, iNAV does nothing and continues. For MC -> FW mission transition: -- guidance uses a straight acceleration segment (no loiter), +- guidance uses a straight acceleration run, - normal waypoint advancement is paused during transition. ### Airspeed-first completion logic MC -> FW: -- completion threshold: `vtol_transition_to_fw_min_airspeed_cm_s` -- if this is `0`, MC->FW uses timer fallback (`mixer_switch_trans_timer`). +- `vtol_transition_to_fw_min_airspeed_cm_s` is the target airspeed. +- If it is `0`, MC->FW uses `mixer_switch_trans_timer` instead. FW -> MC: -- completion threshold: `vtol_transition_to_mc_max_airspeed_cm_s` +- `vtol_transition_to_mc_max_airspeed_cm_s` is the airspeed that must be reached or lower. Timeout: -- `mixer_vtol_transition_airspeed_timeout_ms` can abort transition if condition is not achieved in time. -- This timeout is applied only while the transition is airspeed-controlled (trusted pitot in use). -- If pitot becomes unavailable/unhealthy, completion falls back to `mixer_switch_trans_timer` and this timeout no longer drives the decision. -- For airspeed-first setups, configure a non-zero `mixer_switch_trans_timer` fallback (typical: `40..60`, i.e. `4..6s`) to avoid immediate fallback completion when pitot is unavailable and timer fallback becomes active. +- `mixer_vtol_transition_airspeed_timeout_ms` limits how long iNAV waits for the required airspeed. +- This timeout only matters while pitot airspeed is actually controlling the transition. +- If pitot stops being usable, completion falls back to `mixer_switch_trans_timer` and this timeout no longer decides the outcome. +- For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). -### Dynamic mixer scaling +### Smooth power changes during transition -When `mixer_vtol_transition_dynamic_mixer = ON`, transition progress additionally scales: -- pusher transition contribution, -- vertical lift contribution, -- MC stabilization authority, -- FW transition input authority blend. +When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: +- forward motor power, +- lift motor power, +- multicopter stabilisation strength, +- fixed-wing control strength. -When `mixer_vtol_transition_dynamic_mixer = OFF`, legacy static transition mixing behavior is preserved. +When `mixer_vtol_transition_dynamic_mixer = OFF`, the older static behavior is preserved. -Optional decoupled scaling ramp: +How `mixer_vtol_transition_scale_ramp_time_ms` works: - MC->FW pusher: - - `mixer_vtol_transition_scale_ramp_time_ms > 0`: pusher ramps from `0 -> 100%` over this time, even when trusted pitot is healthy. - - `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): pusher goes to `100%` immediately. -- Lift / MC authority / FW authority handoff: - - trusted pitot available/healthy: follows airspeed-based transition progress. - - `mixer_vtol_transition_scale_ramp_time_ms > 0`: if trusted pitot becomes unavailable/unhealthy, handoff scaling falls back to this ramp timer. - - `mixer_vtol_transition_scale_ramp_time_ms = 0` (default): if trusted pitot is unavailable/unhealthy, handoff scaling falls back to transition progress/timer behavior. -- FW->MC keeps the existing handoff-based scaling behavior. + - `> 0`: forward motor power ramps from `0 -> 100%` over this time, even when pitot is working normally. + - `= 0` (default): forward motor power goes to `100%` immediately. +- Lift motor power, MC stabilisation, and FW control: + - with valid pitot airspeed, they still follow airspeed-based transition progress. + - if pitot stops being usable and this setting is `> 0`, they use this same timer as a backup ramp. + - if pitot stops being usable and this setting is `0`, they fall back to the normal transition timer/progress behavior. +- FW->MC keeps the existing style of smooth handover. Example: - `mixer_switch_trans_timer = 50` (5s fallback completion timer) - `mixer_vtol_transition_scale_ramp_time_ms = 1200` Result: -- in MC->FW, pusher reaches full scale in ~1.2s, -- when trusted pitot is healthy, lift/MC/FW handoff still follows airspeed progress, -- if trusted pitot becomes unavailable/unhealthy, handoff scaling reaches target levels in ~1.2s, -- transition completion still follows airspeed thresholds when pitot is healthy, -- if pitot is unavailable/unhealthy, completion fallback still uses 5s. +- in MC->FW, the forward motor reaches full power in about `1.2s`, +- when pitot is working, lift power and control handover still follow airspeed, +- if pitot stops being usable, the same handover reaches its target in about `1.2s`, +- transition completion still uses airspeed when pitot is working, +- backup completion time is still `5s` if pitot is not usable. ### Example test presets (VTOL ~1.0m wingspan, ~1720g AUW) @@ -413,7 +416,7 @@ These are example starting points for initial testing. They are not universal va #### Test 1 - Legacy-compatible baseline (manual transition check) Goal: -- Verify that the new controller does not change legacy behavior when dynamic scaling is disabled. +- Verify that the new controller does not change legacy behavior when smooth power changes are disabled. - Good first test after flashing. CLI: @@ -432,10 +435,10 @@ What this does: - Uses conservative FW->MC completion threshold. - Disables mission-authorized transition while validating manual behavior. -#### Test 2 - Airspeed-first + dynamic scaling (manual transition tuning) +#### Test 2 - Airspeed-first + smooth power changes (manual transition tuning) Goal: -- Enable the full new behavior: airspeed-first completion and smooth authority/pusher scaling. +- Enable the full new behavior: airspeed-first completion and smooth forward-motor and control handover. CLI: - `set mixer_vtol_manualswitch_autotransition_controller = ON` @@ -453,7 +456,7 @@ CLI: What this does: - MC->FW completes primarily on pitot airspeed (1300 cm/s), with timer fallback only if pitot is unavailable/unhealthy. - FW->MC completes when airspeed drops to 850 cm/s. -- In MC->FW, pusher ramps to full scale in 1.2 s while lift/MC/FW handoff still follows airspeed progress. +- In MC->FW, the forward motor ramps to full power in `1.2s` while lift power and control handover still follow airspeed progress. - The pusher ramp is quick enough (1.2 s) to reduce step torque while still allowing strong acceleration. - Timeout abort protects against staying too long in airspeed-controlled transition without reaching threshold. @@ -475,14 +478,13 @@ CLI: - `set vtol_transition_fw_authority_start_percent = 20` - `set nav_vtol_mission_transition_user_action = USER1` - `set nav_vtol_mission_transition_min_altitude_cm = 1200` -- `set nav_vtol_mission_transition_track_distance_cm = 4000` What this does: - Uses USER1 as the absolute per-waypoint target selector: - USER1 bit clear -> target MC - USER1 bit set -> target FW - Pauses mission progression during transition and resumes after completion. -- Uses straight MC->FW acceleration segment (no loiter) with a 40 m transition track distance. +- Uses a straight MC->FW acceleration segment (no loiter) before the switch to fixed-wing. - Adds a minimum altitude gate (12 m) before mission transition starts. ### Detailed effect of the three percentage settings @@ -490,35 +492,35 @@ What this does: These three settings are active only when `mixer_vtol_transition_dynamic_mixer = ON`. 1. `vtol_transition_lift_end_percent` -- Defines lift throttle scale at transition end. -- MC -> FW: lift goes from `100%` at start to `lift_end_percent` at end. -- FW -> MC: lift goes from `lift_end_percent` at start to `100%` at end. +- Sets how much lift motor power remains at the end of transition. +- MC -> FW: lift power goes from `100%` at start to `lift_end_percent` at the end. +- FW -> MC: lift power goes from `lift_end_percent` at start to `100%` at the end. Example (`vtol_transition_lift_end_percent = 20`): -- MC -> FW at 50% progress: lift scale is about 60%. -- FW -> MC at 50% progress: lift scale is about 60%. +- MC -> FW at 50% progress: lift power is about `60%`. +- FW -> MC at 50% progress: lift power is about `60%`. 2. `vtol_transition_mc_authority_end_percent` -- Defines MC stabilization authority scale at transition end. -- MC -> FW: MC authority goes from `100%` at start to `mc_authority_end_percent` at end. -- FW -> MC: MC authority goes from `mc_authority_end_percent` at start to `100%` at end. +- Sets how much multicopter stabilisation remains at the end of transition. +- MC -> FW: MC stabilisation goes from `100%` at start to `mc_authority_end_percent` at the end. +- FW -> MC: MC stabilisation goes from `mc_authority_end_percent` at start to `100%` at the end. Example (`vtol_transition_mc_authority_end_percent = 30`): -- MC -> FW at 50% progress: MC authority is about 65%. -- FW -> MC at 50% progress: MC authority is about 65%. +- MC -> FW at 50% progress: MC stabilisation is about `65%`. +- FW -> MC at 50% progress: MC stabilisation is about `65%`. 3. `vtol_transition_fw_authority_start_percent` -- Defines FW authority scale at transition start. -- MC -> FW: FW authority goes from `fw_authority_start_percent` at start to `100%` at end. -- FW -> MC: FW authority goes from `100%` at start to `fw_authority_start_percent` at end. +- Sets how much fixed-wing control is already available at the start of transition. +- MC -> FW: fixed-wing control goes from `fw_authority_start_percent` at start to `100%` at the end. +- FW -> MC: fixed-wing control goes from `100%` at start to `fw_authority_start_percent` at the end. Example (`vtol_transition_fw_authority_start_percent = 25`): -- MC -> FW at 50% progress: FW authority is about 62.5%. -- FW -> MC at 50% progress: FW authority is about 62.5%. +- MC -> FW at 50% progress: fixed-wing control is about `62.5%`. +- FW -> MC at 50% progress: fixed-wing control is about `62.5%`. Backward-compatible note: -- `vtol_transition_fw_authority_start_percent = 100` preserves legacy FW authority handoff behavior. -- Lower values provide smoother FW authority ramp-in/out. +- `vtol_transition_fw_authority_start_percent = 100` keeps the older fixed-wing control behavior. +- Lower values bring fixed-wing control in and out more gently. ## Setting Scope (Important) @@ -548,56 +550,52 @@ These are shared system-wide and are not profile-specific: - `vtol_transition_fw_authority_start_percent` - `nav_vtol_mission_transition_user_action` - `nav_vtol_mission_transition_min_altitude_cm` -- `nav_vtol_mission_transition_track_distance_cm` ## CLI Commands (English) Use these commands in CLI (`set ...`, then `save`): - `set mixer_vtol_manualswitch_autotransition_controller = ON|OFF` - - Enables edge-triggered manual transition controller. + - Makes `MIXER TRANSITION` start one automatic transition each time you turn it ON. - `set mixer_vtol_transition_dynamic_mixer = ON|OFF` - - Enables/disables dynamic progress-based scaling. + - Turns smooth transition power changes ON or OFF. - `set vtol_transition_to_fw_min_airspeed_cm_s = ` - Preferred MC -> FW completion threshold (pitot airspeed). - `set mixer_switch_trans_timer = ` - - Timer-based transition duration fallback (used when pitot airspeed is unavailable/unhealthy). + - Backup transition time used when pitot airspeed is not available. - `set vtol_transition_to_mc_max_airspeed_cm_s = ` - FW -> MC completion threshold (pitot airspeed). - `set vtol_fw_to_mc_auto_switch_airspeed_cm_s = ` - - Optional low-airspeed FW protection threshold for manual auto-transition controller (`0` disables). + - Optional low-speed protection threshold for fixed-wing (`0` disables). - `set mixer_vtol_transition_airspeed_timeout_ms = ` - - Transition timeout/abort window. + - How long iNAV waits for required pitot airspeed before aborting. - `set mixer_vtol_transition_scale_ramp_time_ms = ` - - Optional dynamic scaling ramp duration in milliseconds. `0` keeps legacy progress-coupled scaling. `>0` decouples scaling ramp time from completion timing. + - Ramp-up time for the forward motor, and backup ramp time for the other smooth transition power changes. - `set vtol_transition_lift_end_percent = <0..100>` - - Lift scale endpoint for dynamic transition. + - How much lift motor power remains at the end of transition. - `set vtol_transition_mc_authority_end_percent = <0..100>` - - MC authority endpoint for dynamic transition. + - How much multicopter stabilisation remains at the end of transition. - `set vtol_transition_fw_authority_start_percent = <0..100>` - - FW authority start level for dynamic transition. + - How much fixed-wing control is already available at the start of transition. - `set nav_vtol_mission_transition_user_action = OFF|USER1|USER2|USER3|USER4` - - Selects waypoint User Action bit used for mission VTOL target selector (absolute per-waypoint desired state). + - Selects which waypoint USER flag tells iNAV to use MC or FW at each waypoint. - `set nav_vtol_mission_transition_min_altitude_cm = ` - - Optional minimum altitude check before mission transition start (`0` disables). - -- `set nav_vtol_mission_transition_track_distance_cm = ` - - Straight-line transition guidance distance for mission MC -> FW segment. + - Optional minimum altitude before mission transition may start (`0` disables). Mission profile-switch dependency: -- Mission VTOL transition uses the existing profile hot-switch path, so two valid mixer profiles and a configured `MIXER PROFILE 2` mode activation condition are required. +- Mission VTOL transition uses the existing profile-change path, so two valid mixer profiles and a configured `MIXER PROFILE 2` mode activation condition are required. # Notes and Experiences @@ -627,26 +625,26 @@ When pitot is healthy/available, transition progress is airspeed-driven (not tim - progress = `constrain((startAirspeed - airspeed) / (startAirspeed - to_mc_threshold), 0..1)` - completion condition = `airspeed <= to_mc_threshold` -Dynamic mixer scaling (`mixer_vtol_transition_dynamic_mixer = ON`) uses this progress: +Smooth transition power changes (`mixer_vtol_transition_dynamic_mixer = ON`) use this progress: - MC -> FW: - - pusher scale ramps `0 -> 1` - - lift scale ramps `1 -> vtol_transition_lift_end_percent` - - MC authority ramps `1 -> vtol_transition_mc_authority_end_percent` - - FW authority ramps `vtol_transition_fw_authority_start_percent -> 1` + - forward motor power ramps `0 -> 1` + - lift motor power ramps `1 -> vtol_transition_lift_end_percent` + - MC stabilisation ramps `1 -> vtol_transition_mc_authority_end_percent` + - FW control ramps `vtol_transition_fw_authority_start_percent -> 1` - FW -> MC: - - pusher scale ramps `1 -> 0` - - lift scale ramps `vtol_transition_lift_end_percent -> 1` - - MC authority ramps `vtol_transition_mc_authority_end_percent -> 1` - - FW authority ramps `1 -> vtol_transition_fw_authority_start_percent` - -Dynamic scaling splits MC->FW pusher ramp from lift/authority handoff scaling. -For MC->FW, pusher scaling uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, pusher goes full immediately. -Lift / MC authority / FW authority handoff still prefers trusted pitot-based transition progress whenever available. -If trusted pitot becomes unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms > 0`, handoff scaling falls back to that timer-based ramp. -If trusted pitot is unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms = 0`, handoff scaling falls back to transition progress/timer behavior (`mixer_switch_trans_timer`). -FW->MC keeps the existing handoff-based scaling behavior. + - forward motor power ramps `1 -> 0` + - lift motor power ramps `vtol_transition_lift_end_percent -> 1` + - MC stabilisation ramps `vtol_transition_mc_authority_end_percent -> 1` + - FW control ramps `1 -> vtol_transition_fw_authority_start_percent` + +MC->FW uses separate forward-motor ramp-up and control handover behavior. +For MC->FW, forward motor power uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, the motor goes to full power immediately. +Lift motor power, MC stabilisation, and FW control still prefer pitot-based transition progress whenever pitot is working. +If pitot stops being usable and `mixer_vtol_transition_scale_ramp_time_ms > 0`, those other changes fall back to the same timer-based ramp. +If pitot is not usable and `mixer_vtol_transition_scale_ramp_time_ms = 0`, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). +FW->MC keeps the existing style of smooth handover. For transition/pusher motors (`-2.0 < throttle < -1.0`), output is interpolated from idle to target: diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c13536c5971..f66dde618bd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1281,29 +1281,29 @@ groups: field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. If trusted pitot is unavailable/unhealthy and `mixer_vtol_transition_scale_ramp_time_ms = 0`, dynamic scaling also falls back to this transition progress/timer behavior." + description: "Time, in deciseconds (0.1s), that transition motors or servos stay active before iNAV changes to the other mixer profile during an automated VTOL switch. This is also the backup transition time when pitot airspeed is not available. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, the other smooth transition power changes also fall back to this timing." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 max: 200 - name: mixer_vtol_transition_dynamic_mixer - description: "Enables dynamic VTOL transition progress/scaling controller shared by mission-authorized and manual MIXER TRANSITION paths." + description: "Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions." default_value: OFF field: mixer_config.vtolTransitionDynamicMixer type: bool - name: mixer_vtol_manualswitch_autotransition_controller - description: "Enables edge-triggered manual VTOL transition controller for `MIXER TRANSITION` when not in waypoint mission. OFF keeps legacy manual transition behavior. For consistent manual transition semantics, enable this in both mixer profiles." + description: "Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switch moves from OFF to ON, when not in waypoint mission. Turn this ON in both mixer profiles if you want the same behavior in both directions. OFF keeps the older manual switch behavior." default_value: OFF field: mixer_config.manualVtolTransitionController type: bool - name: mixer_vtol_transition_airspeed_timeout_ms - description: "Safety timeout [ms] for airspeed-controlled transitions. If non-zero and required airspeed condition is not met in time, transition aborts instead of force-completing." + description: "How long iNAV will wait for the required pitot airspeed during an airspeed-based transition. If the target airspeed is not reached in time, the transition is aborted. Set to 0 to disable." default_value: 0 field: mixer_config.vtolTransitionAirspeedTimeoutMs min: 0 max: 60000 - name: mixer_vtol_transition_scale_ramp_time_ms - description: "Optional VTOL transition scaling ramp duration [ms]. In MC->FW, pusher scaling uses this timer regardless of pitot availability; if set to 0, pusher goes to full scale immediately. Lift/MC/FW handoff scaling still follows trusted pitot-based transition progress when available; if trusted pitot becomes unavailable/unhealthy, it falls back to this timer. If set to 0, handoff scaling falls back to transition progress/timer behavior." + description: "Ramp-up time [ms] for the forward motor during MC->FW when smooth VTOL transition power changes are ON. `0` gives full forward-motor power immediately. The same timer is also used as a backup for lift motor power, multicopter stabilisation, and fixed-wing control fade if pitot airspeed is lost or unavailable." default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs min: 0 @@ -2655,34 +2655,28 @@ groups: 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. This is an absolute per-waypoint target-state selector and relies on existing mixer profile switching infrastructure (two profiles and valid MIXER PROFILE 2 mode activation condition)." + description: "Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles and a working `MIXER PROFILE 2` mode setup." 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." + description: "Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the 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_vtol_transition_retry_on_airspeed_timeout - description: "If ON, allows one retry for failed airspeed-gated MC->FW auto-transition (mission or RTH head-home): hold position, perform a 360deg yaw scan, align to best measured pitot airspeed heading, and retry transition once." + description: "If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more." default_value: OFF field: general.vtol_transition_retry_on_airspeed_timeout type: bool - name: nav_vtol_transition_fail_action_mc_to_fw - description: "Action executed after a final MC->FW transition failure (after retry logic, if enabled)." + description: "What iNAV should do if MC->FW transition still fails after the final attempt." default_value: "IDLE" field: general.vtol_transition_fail_action_mc_to_fw table: nav_vtol_transition_fail_action_mc_to_fw - name: nav_vtol_transition_fail_action_fw_to_mc - description: "Action executed after a final FW->MC transition failure. LOITER switches to POSHOLD hold at current position (fixed-wing loiter/orbit around current point). FORCE_SWITCH attempts an immediate mixer hot-switch even after failed criteria." + description: "What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met." default_value: "LOITER" field: general.vtol_transition_fail_action_fw_to_mc table: nav_vtol_transition_fail_action_fw_to_mc @@ -4021,37 +4015,37 @@ groups: min: 0 max: 100 - name: vtol_transition_to_fw_min_airspeed_cm_s - description: "Minimum pitot airspeed [cm/s] required to complete MC->FW transition when airspeed is healthy and available. If 0, MC->FW uses timer fallback (`mixer_switch_trans_timer`)." + description: "Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete. If set to 0, iNAV uses the transition timer instead." default_value: 0 field: vtolTransitionToFwMinAirspeed min: 0 max: 20000 - name: vtol_transition_to_mc_max_airspeed_cm_s - description: "Maximum pitot airspeed [cm/s] allowed to complete FW->MC transition when airspeed is healthy and available. If 0, FW->MC uses timer fallback." + description: "When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower. If set to 0, iNAV uses the transition timer instead." default_value: 0 field: vtolTransitionToMcMaxAirspeed min: 0 max: 20000 - name: vtol_fw_to_mc_auto_switch_airspeed_cm_s - description: "Automatic FW->MC protection threshold [cm/s] used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. If set above 0 and valid pitot airspeed is at/below this value while in FW, controller requests FW->MC transition automatically. Set to 0 to disable." + description: "Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV can start FW->MC transition automatically. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable." default_value: 0 field: vtolFwToMcAutoSwitchAirspeed min: 0 max: 20000 - name: vtol_transition_lift_end_percent - description: "Target vertical-lift throttle scale at transition end, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + description: "How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." default_value: 100 field: vtolTransitionLiftEndPercent min: 0 max: 100 - name: vtol_transition_mc_authority_end_percent - description: "Target multicopter stabilization authority scale at transition end, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + description: "How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." default_value: 100 field: vtolTransitionMcAuthorityEndPercent min: 0 max: 100 - name: vtol_transition_fw_authority_start_percent - description: "Initial fixed-wing authority scale at transition start, in percent. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + description: "How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring in fixed-wing control more gently. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." default_value: 100 field: vtolTransitionFwAuthorityStartPercent min: 0 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 10a9618a82a..ef0af043612 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -90,6 +90,7 @@ #define NAV_MIXERAT_RETRY_HEADING_SETTLE_MS 500 #define NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS 6000 #define NAV_MIXERAT_RETRY_MAX_TOTAL_MS 45000 +#define NAV_MIXERAT_MISSION_TRANSITION_TRACK_DISTANCE_CM 4000 /*----------------------------------------------------------- * Compatibility for home position @@ -127,7 +128,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, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -159,7 +160,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .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, .vtol_transition_retry_on_airspeed_timeout = SETTING_NAV_VTOL_TRANSITION_RETRY_ON_AIRSPEED_TIMEOUT_DEFAULT, .vtol_transition_fail_action_mc_to_fw = SETTING_NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_DEFAULT, .vtol_transition_fail_action_fw_to_mc = SETTING_NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_DEFAULT, @@ -2382,8 +2382,7 @@ static void updateMissionTransitionGuidance(void) 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); + calculateFarAwayTarget(&transitionTarget, navMixerATMissionTransition.heading, NAV_MIXERAT_MISSION_TRANSITION_TRACK_DISTANCE_CM); setDesiredPosition(&transitionTarget, navMixerATMissionTransition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return; } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 03fc33f3d81..bff60d96d0e 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -438,7 +438,6 @@ typedef struct navConfig_s { 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 bool vtol_transition_retry_on_airspeed_timeout; // Enables one-shot yaw-scan retry for failed airspeed-gated MC->FW auto-transition uint8_t vtol_transition_fail_action_mc_to_fw; // Action after final MC->FW transition failure uint8_t vtol_transition_fail_action_fw_to_mc; // Action after final FW->MC transition failure From 8fe633734a2dc1670281631dab03fb6ca5d378da Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Fri, 5 Jun 2026 09:08:41 +0300 Subject: [PATCH 22/42] docs(vtol): clarify transition timer roles and fallback behavior --- docs/MixerProfile.md | 9 +++++---- docs/Settings.md | 11 ++++++----- docs/VTOL.md | 13 +++++++++---- src/main/fc/settings.yaml | 10 +++++----- 4 files changed, 25 insertions(+), 18 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index c966cf8c501..107bd1ba396 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -146,11 +146,11 @@ When valid pitot airspeed is available, iNAV uses airspeed to decide when the tr If pitot is not available, not healthy, or the threshold is set to `0`, iNAV uses `mixer_switch_trans_timer` instead. Ground speed is not used for transition completion/progress. -Optional timeout: +The three timer settings do different jobs: -- `mixer_vtol_transition_airspeed_timeout_ms` limits how long iNAV waits for the required airspeed. -- This timeout is only used while pitot airspeed is actually controlling the transition. -- If pitot is lost, iNAV falls back to `mixer_switch_trans_timer` and this timeout no longer decides the outcome. +- `mixer_switch_trans_timer` is the original VTOL transition timer. It is still the backup completion timer when trusted pitot airspeed is not being used. +- `mixer_vtol_transition_airspeed_timeout_ms` is only a maximum wait time for the required airspeed while pitot is still usable. It does not complete the transition by itself; it aborts that airspeed-controlled attempt. +- If pitot becomes unavailable during transition, iNAV stops using the airspeed timeout and falls back to `mixer_switch_trans_timer`. - For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). ### Smooth power changes during transition (optional) @@ -170,6 +170,7 @@ How `mixer_vtol_transition_scale_ramp_time_ms` works: - MC->FW pusher: - `> 0`: forward motor power ramps from `0 -> 100%` over this time, even when pitot is working normally. - `= 0` (default): forward motor power goes to `100%` immediately. +- This timer does not decide when the transition completes. - Lift motor power, MC stabilisation, and FW control: - with valid pitot airspeed, they still follow transition progress based on airspeed. - if pitot stops being usable and this setting is `> 0`, they use this same timer as a backup ramp. diff --git a/docs/Settings.md b/docs/Settings.md index 14062ec4a33..bf5aed2876f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3202,7 +3202,7 @@ If enabled, control_profile_index will follow mixer_profile index. Set to OFF(de ### mixer_switch_trans_timer -Time, in deciseconds (0.1s), that transition motors or servos stay active before iNAV changes to the other mixer profile during an automated VTOL switch. This is also the backup transition time when pitot airspeed is not available. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, the other smooth transition power changes also fall back to this timing. +Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing. | Default | Min | Max | | --- | --- | --- | @@ -3222,7 +3222,7 @@ Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switc ### mixer_vtol_transition_airspeed_timeout_ms -How long iNAV will wait for the required pitot airspeed during an airspeed-based transition. If the target airspeed is not reached in time, the transition is aborted. Set to 0 to disable. +Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. | Default | Min | Max | | --- | --- | --- | @@ -3242,7 +3242,7 @@ Turns on smooth VTOL transition power changes. This affects forward motor ramp-u ### mixer_vtol_transition_scale_ramp_time_ms -Ramp-up time [ms] for the forward motor during MC->FW when smooth VTOL transition power changes are ON. `0` gives full forward-motor power immediately. The same timer is also used as a backup for lift motor power, multicopter stabilisation, and fixed-wing control fade if pitot airspeed is lost or unavailable. +When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable. | Default | Min | Max | | --- | --- | --- | @@ -7118,7 +7118,7 @@ How much multicopter stabilisation remains at the end of transition, in percent. ### vtol_transition_to_fw_min_airspeed_cm_s -Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete. If set to 0, iNAV uses the transition timer instead. +Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. | Default | Min | Max | | --- | --- | --- | @@ -7128,7 +7128,7 @@ Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered comp ### vtol_transition_to_mc_max_airspeed_cm_s -When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower. If set to 0, iNAV uses the transition timer instead. +When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. | Default | Min | Max | | --- | --- | --- | @@ -7289,3 +7289,4 @@ Defines rotation rate on YAW axis that UAV will try to archive on max. stick def | 20 | 1 | 180 | --- + diff --git a/docs/VTOL.md b/docs/VTOL.md index 484ce28c47b..e4093197c6b 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -367,15 +367,16 @@ For MC -> FW mission transition: MC -> FW: - `vtol_transition_to_fw_min_airspeed_cm_s` is the target airspeed. -- If it is `0`, MC->FW uses `mixer_switch_trans_timer` instead. +- If pitot stops being usable, or if this is `0`, MC->FW uses `mixer_switch_trans_timer` instead. FW -> MC: - `vtol_transition_to_mc_max_airspeed_cm_s` is the airspeed that must be reached or lower. +- If pitot stops being usable, or if this is `0`, FW->MC uses `mixer_switch_trans_timer` instead. Timeout: -- `mixer_vtol_transition_airspeed_timeout_ms` limits how long iNAV waits for the required airspeed. -- This timeout only matters while pitot airspeed is actually controlling the transition. -- If pitot stops being usable, completion falls back to `mixer_switch_trans_timer` and this timeout no longer decides the outcome. +- `mixer_switch_trans_timer` is the original VTOL transition timer. It is still the backup completion timer when trusted pitot airspeed is not being used. +- `mixer_vtol_transition_airspeed_timeout_ms` is only a maximum wait time for the required airspeed while pitot is still usable. It does not complete the transition by itself; it aborts that airspeed-controlled attempt. +- If pitot stops being usable, iNAV stops using the airspeed timeout and falls back to `mixer_switch_trans_timer`. - For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). ### Smooth power changes during transition @@ -388,6 +389,9 @@ When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: When `mixer_vtol_transition_dynamic_mixer = OFF`, the older static behavior is preserved. +`mixer_vtol_transition_scale_ramp_time_ms` always controls the MC->FW forward-motor ramp when this feature is ON. +It does not decide when the transition completes. + How `mixer_vtol_transition_scale_ramp_time_ms` works: - MC->FW pusher: - `> 0`: forward motor power ramps from `0 -> 100%` over this time, even when pitot is working normally. @@ -641,6 +645,7 @@ Smooth transition power changes (`mixer_vtol_transition_dynamic_mixer = ON`) use MC->FW uses separate forward-motor ramp-up and control handover behavior. For MC->FW, forward motor power uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, the motor goes to full power immediately. +This timer does not decide when the transition completes. Lift motor power, MC stabilisation, and FW control still prefer pitot-based transition progress whenever pitot is working. If pitot stops being usable and `mixer_vtol_transition_scale_ramp_time_ms > 0`, those other changes fall back to the same timer-based ramp. If pitot is not usable and `mixer_vtol_transition_scale_ramp_time_ms = 0`, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f66dde618bd..2ed28bb6509 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1281,7 +1281,7 @@ groups: field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "Time, in deciseconds (0.1s), that transition motors or servos stay active before iNAV changes to the other mixer profile during an automated VTOL switch. This is also the backup transition time when pitot airspeed is not available. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, the other smooth transition power changes also fall back to this timing." + description: "Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 @@ -1297,13 +1297,13 @@ groups: field: mixer_config.manualVtolTransitionController type: bool - name: mixer_vtol_transition_airspeed_timeout_ms - description: "How long iNAV will wait for the required pitot airspeed during an airspeed-based transition. If the target airspeed is not reached in time, the transition is aborted. Set to 0 to disable." + description: "Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable." default_value: 0 field: mixer_config.vtolTransitionAirspeedTimeoutMs min: 0 max: 60000 - name: mixer_vtol_transition_scale_ramp_time_ms - description: "Ramp-up time [ms] for the forward motor during MC->FW when smooth VTOL transition power changes are ON. `0` gives full forward-motor power immediately. The same timer is also used as a backup for lift motor power, multicopter stabilisation, and fixed-wing control fade if pitot airspeed is lost or unavailable." + description: "When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable." default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs min: 0 @@ -4015,13 +4015,13 @@ groups: min: 0 max: 100 - name: vtol_transition_to_fw_min_airspeed_cm_s - description: "Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete. If set to 0, iNAV uses the transition timer instead." + description: "Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted." default_value: 0 field: vtolTransitionToFwMinAirspeed min: 0 max: 20000 - name: vtol_transition_to_mc_max_airspeed_cm_s - description: "When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower. If set to 0, iNAV uses the transition timer instead." + description: "When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted." default_value: 0 field: vtolTransitionToMcMaxAirspeed min: 0 From 1d2cd6f15ffcb024ae617d9b2b71be95931e2cac Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Fri, 5 Jun 2026 09:31:54 +0300 Subject: [PATCH 23/42] vtol: support overlap mapping with manual transition controller --- docs/MixerProfile.md | 22 +++++++++++++--------- docs/VTOL.md | 14 +++++++------- src/main/flight/mixer_profile.c | 3 ++- 3 files changed, 22 insertions(+), 17 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 107bd1ba396..108a784cf55 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -50,21 +50,20 @@ In Active Modes: There are two separate manual paths: -- `MIXER PROFILE 2` is still a direct manual profile switch. +- `MIXER PROFILE 2` is still a direct manual profile switch when `MIXER TRANSITION` is OFF. - `MIXER TRANSITION` starts the smooth automatic transition sequence when `mixer_vtol_manualswitch_autotransition_controller = ON`. +- If both are ON together while the automatic transition controller is enabled, the controller temporarily owns the profile switching. When `MIXER TRANSITION` turns OFF again, direct `MIXER PROFILE 2` switching becomes active again. -Recommended 3-position switch example: +3-position switch example: - This example assumes the usual VTOL order used in this document: - Profile 1 = FW - Profile 2 = MC -- Use a dedicated 3-position mapping: +- One supported mapping is: - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) - Pos2 = Transition request (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) - Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. -- Avoid a switch position that turns ON both `MIXER PROFILE 2` and `MIXER TRANSITION`. -- If both are ON together, iNAV may switch profile immediately instead of running the smooth transition. - If you intentionally swap the profile order, keep the same idea and swap the FW and MC end positions. ## Servo @@ -103,8 +102,9 @@ Profile files Switching is not available until the runtime sensor calibration is `mixer_profile` 1 will be used as default, `mixer_profile` 2 will be used when the `MIXER PROFILE 2` mode box is activated. Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. -The example below is a **legacy manual switch** example, where `MIXER TRANSITION` is used as a live transition input and `mixer_vtol_manualswitch_autotransition_controller = OFF`. -It is not the recommended mapping for the newer automatic transition controller. +Another supported mapping is where one switch position turns ON both `MIXER PROFILE 2` and `MIXER TRANSITION`. +With `mixer_vtol_manualswitch_autotransition_controller = OFF`, `MIXER TRANSITION` is used as a live transition input. +With `mixer_vtol_manualswitch_autotransition_controller = ON`, that same overlap position is used as a controller-owned transition position. ![Alt text](Screenshots/mixer_profile.png) @@ -114,8 +114,12 @@ It is not the recommended mapping for the newer automatic transition controller. It is also possible to set it as 4 state switch by adding FW(profile1) with transition on. -If `mixer_vtol_manualswitch_autotransition_controller = ON`, do **not** use this overlap style where `MIXER PROFILE 2` and `MIXER TRANSITION` are ON together. -For the newer smooth automatic transition behavior, use the dedicated 3-position mapping shown earlier in this document. +If `mixer_vtol_manualswitch_autotransition_controller = ON` and this overlap position is active: +- the smooth transition controller runs while `MIXER TRANSITION` stays ON +- direct `MIXER PROFILE 2` switching is deferred during that time +- when `MIXER TRANSITION` turns OFF, `MIXER PROFILE 2` again decides which stable mixer profile should be active + +This overlap style is supported too. ## Automated Transition This feature is mainly used for RTH and failsafe. diff --git a/docs/VTOL.md b/docs/VTOL.md index e4093197c6b..6d692489192 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -180,10 +180,11 @@ You must also assign the tilting servos values using the MAX values. If you don | :-- | :-- | :-- | | Profile1(FW) with transition off | Profile2(MC) with transition on | Profile2(MC) with transition off | -- This is a **legacy manual switch** example, where `MIXER TRANSITION` is used as a live transition input. -- It is suitable for the older manual behavior with `mixer_vtol_manualswitch_autotransition_controller = OFF`. -- If you use `mixer_vtol_manualswitch_autotransition_controller = ON`, do **not** use this overlap mapping. -- For the newer smooth automatic transition behavior, use the dedicated 3-position mapping described later in this document, where `MIXER PROFILE 2` and `MIXER TRANSITION` are not ON together. +- This is one supported mapping, where one switch position turns ON both `MIXER PROFILE 2` and `MIXER TRANSITION`. +- With `mixer_vtol_manualswitch_autotransition_controller = OFF`, `MIXER TRANSITION` is used as a live transition input. +- With `mixer_vtol_manualswitch_autotransition_controller = ON`, that same overlap position is used as a controller-owned transition position. +- While both are ON, the smooth transition controller runs and direct `MIXER PROFILE 2` switching is deferred. +- When `MIXER TRANSITION` turns OFF again, `MIXER PROFILE 2` once more decides which stable mixer profile should be active. - Profile file switching becomes available after completing the runtime sensor calibration (15-30s after booting). And It is **not available** when a navigation mode or position hold is active. @@ -339,13 +340,12 @@ Operational example: - reverse the order for FW->MC Important RC mapping constraint: -- Use a dedicated 3-position mapping where: +- One supported mapping is: - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) - Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. -- Avoid a switch position that turns ON both `MIXER PROFILE 2` and `MIXER TRANSITION`. -- If both are ON together, iNAV may switch profile immediately instead of running the smooth transition. +- Another supported mapping is the overlap version: while both `MIXER PROFILE 2` and `MIXER TRANSITION` are ON, the transition controller owns the switching until `MIXER TRANSITION` turns OFF again. ### Mission-authorized transition semantics diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 7f477b9b6a4..f24363ca997 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -540,6 +540,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) const bool missionActive = (navGetCurrentStateFlags() & NAV_AUTO_WP) != 0; const bool manualControllerConfigured = currentMixerConfig.manualVtolTransitionController && !missionActive; bool manualControllerEnabled = manualControllerConfigured || manualTransitionSessionLatched; + const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; const bool mixerProfileModePresent = isModeActivationConditionPresent(BOXMIXERPROFILE); const int requestedProfileIndex = IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1; @@ -559,7 +560,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) { - if (mixerProfileModePresent) { + if (mixerProfileModePresent && !transitionControllerOwnsProfileSwitch) { outputProfileHotSwitch(requestedProfileIndex); } } From cc85129038fc08a4eb1d6dd41e54b011cb6bfee3 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Fri, 5 Jun 2026 15:37:57 +0300 Subject: [PATCH 24/42] vtol: latch fw-to-mc protection and reuse multirotor platform helper --- docs/MixerProfile.md | 3 ++- docs/Settings.md | 2 +- docs/VTOL.md | 4 ++-- src/main/fc/settings.yaml | 2 +- src/main/flight/mixer.h | 7 +++++++ src/main/flight/mixer_profile.c | 26 ++++++++++++++++++++++---- src/main/navigation/navigation.c | 4 +--- 7 files changed, 36 insertions(+), 12 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 108a784cf55..a55f8d10962 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -37,7 +37,7 @@ If `mixer_vtol_manualswitch_autotransition_controller = ON`, `MIXER TRANSITION` - Leaving the switch ON does not keep restarting the transition. - To start another transition, turn the switch OFF and then ON again. - If you turn the switch OFF before the profile change happens, that transition request is cancelled. -- Optional extra protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can automatically start FW->MC if airspeed gets too low. +- Optional extra protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can automatically start FW->MC if airspeed gets too low. After that switch, iNAV stays in MC until you deliberately command another manual profile change. This behavior is controlled by `mixer_vtol_manualswitch_autotransition_controller`. Turn it ON in both mixer profiles if you want the same switch behavior in both directions. @@ -53,6 +53,7 @@ There are two separate manual paths: - `MIXER PROFILE 2` is still a direct manual profile switch when `MIXER TRANSITION` is OFF. - `MIXER TRANSITION` starts the smooth automatic transition sequence when `mixer_vtol_manualswitch_autotransition_controller = ON`. - If both are ON together while the automatic transition controller is enabled, the controller temporarily owns the profile switching. When `MIXER TRANSITION` turns OFF again, direct `MIXER PROFILE 2` switching becomes active again. +- If low-speed protection switches the model from FW to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. 3-position switch example: diff --git a/docs/Settings.md b/docs/Settings.md index bf5aed2876f..76ef09a71e5 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -7078,7 +7078,7 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, ### vtol_fw_to_mc_auto_switch_airspeed_cm_s -Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV can start FW->MC transition automatically. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. +Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md index 6d692489192..396deb0c5e3 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -325,7 +325,7 @@ With `mixer_vtol_manualswitch_autotransition_controller = ON`: - Leaving the switch ON does not keep restarting the transition. - To start another transition, turn the switch OFF and then ON again. - If you turn the switch OFF before the profile change happens, that transition request is cancelled. -- Optional extra protection: set `vtol_fw_to_mc_auto_switch_airspeed_cm_s > 0` if you want FW->MC to start automatically when pitot airspeed becomes too low. +- Optional extra protection: set `vtol_fw_to_mc_auto_switch_airspeed_cm_s > 0` if you want FW->MC to start automatically when pitot airspeed becomes too low. After that switch, iNAV stays in MC until you deliberately command another manual profile change. With `mixer_vtol_manualswitch_autotransition_controller = OFF`: - the older manual behavior is preserved. @@ -575,7 +575,7 @@ Use these commands in CLI (`set ...`, then `save`): - FW -> MC completion threshold (pitot airspeed). - `set vtol_fw_to_mc_auto_switch_airspeed_cm_s = ` - - Optional low-speed protection threshold for fixed-wing (`0` disables). + - Optional low-speed protection threshold for fixed-wing. After it switches to MC, iNAV stays in MC until you deliberately command another manual profile change (`0` disables). - `set mixer_vtol_transition_airspeed_timeout_ms = ` - How long iNAV waits for required pitot airspeed before aborting. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2ed28bb6509..e9045323abd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4027,7 +4027,7 @@ groups: min: 0 max: 20000 - name: vtol_fw_to_mc_auto_switch_airspeed_cm_s - description: "Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV can start FW->MC transition automatically. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable." + description: "Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable." default_value: 0 field: vtolFwToMcAutoSwitchAirspeed min: 0 diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 12688bd2c09..584c05b0888 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -43,6 +43,13 @@ typedef enum { PLATFORM_BOAT = 5 } flyingPlatformType_e; +static inline bool isMultirotorTypePlatform(const flyingPlatformType_e platformType) +{ + return platformType == PLATFORM_MULTIROTOR || + platformType == PLATFORM_TRICOPTER || + platformType == PLATFORM_HELICOPTER; +} + typedef enum { OUTPUT_MODE_AUTO = 0, diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index f24363ca997..bdd322e3301 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -43,6 +43,7 @@ int nextMixerProfileIndex; static bool manualTransitionModeWasActive; static bool manualTransitionReadyForEdge = true; static bool manualTransitionSessionLatched; +static bool manualFwToMcProtectionLatched; PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 4); @@ -406,9 +407,7 @@ static bool missionTransitionToMultirotorTypeConfigured(void) } const flyingPlatformType_e nextPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; - return nextPlatformType == PLATFORM_MULTIROTOR || - nextPlatformType == PLATFORM_TRICOPTER || - nextPlatformType == PLATFORM_HELICOPTER; + return isMultirotorTypePlatform(nextPlatformType); } bool checkMixerATRequired(mixerProfileATRequest_e required_action) @@ -543,24 +542,40 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; const bool mixerProfileModePresent = isModeActivationConditionPresent(BOXMIXERPROFILE); const int requestedProfileIndex = IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1; + const bool requestedMultirotorProfile = mixerProfileModePresent && + isMultirotorTypePlatform(mixerConfigByIndex(requestedProfileIndex)->platformType); + // If low-speed protection already moved the model back to MC, keep direct + // switching from forcing FW again until the pilot makes a new manual choice. + const bool fwToMcProtectionOwnsProfileSwitch = manualFwToMcProtectionLatched && + STATE(MULTIROTOR) && + !requestedMultirotorProfile; if (manualControllerConfigured && transitionModeRisingEdge) { manualTransitionSessionLatched = true; } + if (transitionModeRisingEdge) { + manualFwToMcProtectionLatched = false; + } + if (transitionModeFallingEdge) { manualTransitionSessionLatched = false; } + if (requestedMultirotorProfile || (!mixerAT_inuse && !STATE(MULTIROTOR))) { + manualFwToMcProtectionLatched = false; + } + if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { abortTransition(false); manualTransitionSessionLatched = false; + manualFwToMcProtectionLatched = false; mixerAT_inuse = false; } if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) { - if (mixerProfileModePresent && !transitionControllerOwnsProfileSwitch) { + if (mixerProfileModePresent && !transitionControllerOwnsProfileSwitch && !fwToMcProtectionOwnsProfileSwitch) { outputProfileHotSwitch(requestedProfileIndex); } } @@ -573,6 +588,9 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) checkMixerATRequired(MIXERAT_REQUEST_MANUAL_TO_MC)) { mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_MC); mixerAT_inuse = mixerATIsActive(); + if (mixerAT_inuse || STATE(MULTIROTOR)) { + manualFwToMcProtectionLatched = true; + } } if (!manualControllerEnabled) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ef0af043612..7d7120dd85c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2070,9 +2070,7 @@ static uint16_t missionUserActionMask(const navMissionUserAction_e userAction) static bool isMissionTransitionToMultirotorType(const flyingPlatformType_e platformType) { - return platformType == PLATFORM_MULTIROTOR || - platformType == PLATFORM_TRICOPTER || - platformType == PLATFORM_HELICOPTER; + return isMultirotorTypePlatform(platformType); } #ifdef USE_PITOT From a32326a9faf1d871da2e7e7bbb66ba441ba079bf Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Sun, 7 Jun 2026 12:32:28 +0300 Subject: [PATCH 25/42] feat(vtol): add INPUT_AUTOTRANSITION_TARGET_STABILIZED* input source calculated by fwAuthorityScale define USE_AUTO_TRANSITION and end keep auto transition logic off for 512 KB targets to preserve flash space --- docs/MixerProfile.md | 12 +- docs/Navigation.md | 2 + docs/Settings.md | 30 +- docs/VTOL.md | 11 + src/main/blackbox/blackbox.c | 7 + src/main/fc/config.c | 2 + src/main/fc/config.h | 2 + src/main/fc/fc_msp_box.c | 5 + src/main/fc/settings.yaml | 45 ++- src/main/flight/mixer.c | 17 +- src/main/flight/mixer_profile.c | 117 ++++++- src/main/flight/mixer_profile.h | 16 + src/main/flight/pid.c | 425 ++++++++++++++++++++++- src/main/flight/pid.h | 8 +- src/main/flight/servos.c | 112 ++++++ src/main/flight/servos.h | 11 + src/main/navigation/navigation.c | 76 ++++ src/main/navigation/navigation.h | 4 + src/main/navigation/navigation_private.h | 2 + src/main/programming/logic_condition.c | 21 ++ src/main/programming/logic_condition.h | 5 + src/main/target/common.h | 4 +- 22 files changed, 894 insertions(+), 40 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index a55f8d10962..25b05fda5b2 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -6,7 +6,9 @@ A MixerProfile is a set of motor mixer, servo-mixer and platform type configurat Not limited to VTOL. air/land/sea mixed vehicle is also achievable with this feature. Model behaves according to current mixer_profile's platform_type and configured custom motor/servo mixer -Currently two profiles are supported on targets other than F411(due to resource constraints on F411). i.e VTOL transition is not available on F411. +Two mixer profiles and smooth VTOL auto-transition are available only on targets with enough flash space. +In standard INAV builds this means targets with more than 512 KB flash, compiled with `USE_AUTO_TRANSITION`. +Targets with 512 KB flash keep the older single-profile / legacy transition behavior and do not include the smooth auto-transition settings. For VTOL setup. one mixer_profile is used for multi-rotor(MR) and the other is used for fixed-wing(FW) By default, switching between profiles requires reboot to take affect. However, using the RC mode: `MIXER PROFILE 2` will allow in flight switching for things like VTOL operation @@ -139,6 +141,8 @@ If `mixer_automated_switch = OFF` in all mixer profiles, automated VTOL transiti Manual `MIXER TRANSITION` and mission-requested VTOL transition both use the same internal transition controller. That means the same airspeed checks, timer fallback, and smooth power changes are reused in both cases. +This section applies only to targets with more than 512 KB flash, compiled with `USE_AUTO_TRANSITION`. + Direct manual `MIXER PROFILE 2` switching is still a separate path when you want an immediate profile change. ### Airspeed-first completion @@ -168,7 +172,11 @@ When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: - fixed-wing control strength. Default is OFF to preserve existing behavior. -With it ON, `vtol_transition_fw_authority_start_percent = 100` keeps the old fixed-wing control behavior. Lower values bring fixed-wing control in more gently. +With it ON, you can configure `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules in the MC mixer profile. +During MC->FW they drive the selected servo outputs from the target FW controller before the hot-switch. +During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. +These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. +`INPUT_MIXER_TRANSITION` remains available for transition-progress servo movement such as tilt or helper servos. How `mixer_vtol_transition_scale_ramp_time_ms` works: diff --git a/docs/Navigation.md b/docs/Navigation.md index e3598cf5fc0..f73d894c981 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -105,6 +105,8 @@ Parameters: ### Mission VTOL transition using existing User Actions Mission VTOL transition can be requested. +This is available only on targets with more than 512 KB flash, compiled with `USE_AUTO_TRANSITION`. +Targets with 512 KB flash do not include these mission VTOL transition settings. Configuration: diff --git a/docs/Settings.md b/docs/Settings.md index 76ef09a71e5..90ab68a7527 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3212,7 +3212,7 @@ Original VTOL transition timer, still used as the backup completion time. If tru ### mixer_vtol_manualswitch_autotransition_controller -Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switch moves from OFF to ON, when not in waypoint mission. Turn this ON in both mixer profiles if you want the same behavior in both directions. OFF keeps the older manual switch behavior. +Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switch moves from OFF to ON, when not in waypoint mission. Turn this ON in both mixer profiles if you want the same behavior in both directions. OFF keeps the older manual switch behavior. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -3222,7 +3222,7 @@ Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switc ### mixer_vtol_transition_airspeed_timeout_ms -Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. +Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -3232,7 +3232,7 @@ Maximum wait time [ms] for the required pitot airspeed during an airspeed-contro ### mixer_vtol_transition_dynamic_mixer -Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. +Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -3242,7 +3242,7 @@ Turns on smooth VTOL transition power changes. This affects forward motor ramp-u ### mixer_vtol_transition_scale_ramp_time_ms -When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable. +When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -4669,7 +4669,7 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT ### nav_vtol_mission_transition_min_altitude_cm -Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the altitude check. +Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the altitude check. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -4679,7 +4679,7 @@ Do not start a mission-requested VTOL transition below this altitude [cm]. Set t ### nav_vtol_mission_transition_user_action -Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles and a working `MIXER PROFILE 2` mode setup. +Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles, a working `MIXER PROFILE 2` mode setup, and a target with more than 512 KB flash. | Allowed Values | | | --- | --- | @@ -4693,7 +4693,7 @@ Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode ### nav_vtol_transition_fail_action_fw_to_mc -What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. +What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. Available only on targets with more than 512 KB flash. | Allowed Values | | | --- | --- | @@ -4707,7 +4707,7 @@ What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near ### nav_vtol_transition_fail_action_mc_to_fw -What iNAV should do if MC->FW transition still fails after the final attempt. +What iNAV should do if MC->FW transition still fails after the final attempt. Available only on targets with more than 512 KB flash. | Allowed Values | | | --- | --- | @@ -4720,7 +4720,7 @@ What iNAV should do if MC->FW transition still fails after the final attempt. ### nav_vtol_transition_retry_on_airspeed_timeout -If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. +If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7078,7 +7078,7 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, ### vtol_fw_to_mc_auto_switch_airspeed_cm_s -Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. +Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7088,7 +7088,7 @@ Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to th ### vtol_transition_fw_authority_start_percent -How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring in fixed-wing control more gently. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. +How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring it in and out more gently. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7098,7 +7098,7 @@ How much fixed-wing control is available at the start of transition, in percent. ### vtol_transition_lift_end_percent -How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. +How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7108,7 +7108,7 @@ How much lift motor power remains at the end of transition, in percent. `100` ke ### vtol_transition_mc_authority_end_percent -How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. +How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7118,7 +7118,7 @@ How much multicopter stabilisation remains at the end of transition, in percent. ### vtol_transition_to_fw_min_airspeed_cm_s -Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. +Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7128,7 +7128,7 @@ Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered comp ### vtol_transition_to_mc_max_airspeed_cm_s -When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. +When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md index 396deb0c5e3..9895f1a03d5 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -297,6 +297,10 @@ If you set `mixer_automated_switch` to `OFF` for all mixer profiles (the default ## Unified VTOL Transition Controller (Manual + Mission) +This feature is available only on targets with more than 512 KB flash. +In standard INAV builds those targets are compiled with `USE_AUTO_TRANSITION`. +Targets with 512 KB flash keep the older VTOL mixer transition behavior and do not include the smooth auto-transition settings. + INAV now uses one internal VTOL transition controller for both: - manual `MIXER TRANSITION` requests, and - mission-authorized VTOL transitions. @@ -388,6 +392,11 @@ When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: - fixed-wing control strength. When `mixer_vtol_transition_dynamic_mixer = OFF`, the older static behavior is preserved. +When it is ON, you can configure `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules in the MC mixer profile. +During MC->FW they drive the selected servo outputs from the target FW controller before the hot-switch. +During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. +These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. +`INPUT_MIXER_TRANSITION` remains available for transition-progress servo movement such as tilt or helper servos. `mixer_vtol_transition_scale_ramp_time_ms` always controls the MC->FW forward-motor ramp when this feature is ON. It does not decide when the transition completes. @@ -517,6 +526,8 @@ Example (`vtol_transition_mc_authority_end_percent = 30`): - Sets how much fixed-wing control is already available at the start of transition. - MC -> FW: fixed-wing control goes from `fw_authority_start_percent` at start to `100%` at the end. - FW -> MC: fixed-wing control goes from `100%` at start to `fw_authority_start_percent` at the end. +- During MC -> FW, this same setting also scales `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules configured in the MC mixer profile. +- During FW -> MC, the same setting scales down the matching FW servo stabilisation on the outputs marked by those MC mixer rules. Example (`vtol_transition_fw_authority_start_percent = 25`): - MC -> FW at 50% progress: fixed-wing control is about `62.5%`. diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 7ad94f88263..4414fed501d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1367,10 +1367,13 @@ static void writeSlowFrame(void) */ static void loadSlowState(blackboxSlowState_t *slow) { +#ifdef USE_AUTO_TRANSITION boxBitmask_t reportedRcModeFlags = rcModeActivationMask; +#endif slow->activeWpNumber = getActiveWpNumber(); +#ifdef USE_AUTO_TRANSITION // Keep these two mode bits aligned with actual VTOL state/profile activity for status reporting. if (isMixerProfile2ModeReportedActive()) { bitArraySet(reportedRcModeFlags.bits, BOXMIXERPROFILE); @@ -1386,6 +1389,10 @@ static void loadSlowState(blackboxSlowState_t *slow) slow->rcModeFlags = reportedRcModeFlags.bits[0]; // first 32 bits of boxId_e slow->rcModeFlags2 = reportedRcModeFlags.bits[1]; // remaining bits of boxId_e +#else + slow->rcModeFlags = rcModeActivationMask.bits[0]; // first 32 bits of boxId_e + slow->rcModeFlags2 = rcModeActivationMask.bits[1]; // remaining bits of boxId_e +#endif // Also log Nav auto enabled flight modes rather than just those selected by boxmode if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) { diff --git a/src/main/fc/config.c b/src/main/fc/config.c index f5cafa62ae7..f9254e41aa1 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -117,12 +117,14 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .i2c_speed = SETTING_I2C_SPEED_DEFAULT, #endif .throttle_tilt_compensation_strength = SETTING_THROTTLE_TILT_COMP_STR_DEFAULT, // 0-100, 0 - disabled +#ifdef USE_AUTO_TRANSITION .vtolTransitionToFwMinAirspeed = SETTING_VTOL_TRANSITION_TO_FW_MIN_AIRSPEED_CM_S_DEFAULT, .vtolTransitionToMcMaxAirspeed = SETTING_VTOL_TRANSITION_TO_MC_MAX_AIRSPEED_CM_S_DEFAULT, .vtolFwToMcAutoSwitchAirspeed = SETTING_VTOL_FW_TO_MC_AUTO_SWITCH_AIRSPEED_CM_S_DEFAULT, .vtolTransitionLiftEndPercent = SETTING_VTOL_TRANSITION_LIFT_END_PERCENT_DEFAULT, .vtolTransitionMcAuthorityEndPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_END_PERCENT_DEFAULT, .vtolTransitionFwAuthorityStartPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_START_PERCENT_DEFAULT, +#endif .craftName = SETTING_NAME_DEFAULT, .pilotName = SETTING_NAME_DEFAULT ); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 1478754f350..fd433e9e657 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -78,12 +78,14 @@ typedef struct systemConfig_s { uint8_t i2c_speed; #endif uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. +#ifdef USE_AUTO_TRANSITION uint16_t vtolTransitionToFwMinAirspeed; uint16_t vtolTransitionToMcMaxAirspeed; uint16_t vtolFwToMcAutoSwitchAirspeed; uint8_t vtolTransitionLiftEndPercent; uint8_t vtolTransitionMcAuthorityEndPercent; uint8_t vtolTransitionFwAuthorityStartPercent; +#endif char craftName[MAX_NAME_LENGTH + 1]; char pilotName[MAX_NAME_LENGTH + 1]; } systemConfig_t; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index ba6f3f69593..bfcfcbf6099 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -447,8 +447,13 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)), BOXMULTIFUNCTION); #endif #if (MAX_MIXER_PROFILE_COUNT > 1) +#ifdef USE_AUTO_TRANSITION CHECK_ACTIVE_BOX(IS_ENABLED(isMixerProfile2ModeReportedActive()), BOXMIXERPROFILE); CHECK_ACTIVE_BOX(IS_ENABLED(isMixerTransitionModeReportedActive()), BOXMIXERTRANSITION); +#else + CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION); +#endif #endif CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e9045323abd..a65788dd1f0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1287,23 +1287,27 @@ groups: min: 0 max: 200 - name: mixer_vtol_transition_dynamic_mixer - description: "Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions." + description: "Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: OFF field: mixer_config.vtolTransitionDynamicMixer type: bool - name: mixer_vtol_manualswitch_autotransition_controller - description: "Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switch moves from OFF to ON, when not in waypoint mission. Turn this ON in both mixer profiles if you want the same behavior in both directions. OFF keeps the older manual switch behavior." + description: "Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switch moves from OFF to ON, when not in waypoint mission. Turn this ON in both mixer profiles if you want the same behavior in both directions. OFF keeps the older manual switch behavior. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: OFF field: mixer_config.manualVtolTransitionController type: bool - name: mixer_vtol_transition_airspeed_timeout_ms - description: "Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable." + description: "Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: 0 field: mixer_config.vtolTransitionAirspeedTimeoutMs min: 0 max: 60000 - name: mixer_vtol_transition_scale_ramp_time_ms - description: "When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable." + description: "When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs min: 0 @@ -2655,28 +2659,33 @@ groups: field: general.flags.waypoint_mission_restart table: nav_wp_mission_restart - name: nav_vtol_mission_transition_user_action - description: "Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles and a working `MIXER PROFILE 2` mode setup." + description: "Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles, a working `MIXER PROFILE 2` mode setup, and a target with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: "OFF" field: general.vtol_mission_transition_user_action table: nav_wp_user_action - name: nav_vtol_mission_transition_min_altitude_cm - description: "Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the altitude check." + description: "Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the altitude check. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: 0 field: general.vtol_mission_transition_min_altitude min: 0 max: 50000 - name: nav_vtol_transition_retry_on_airspeed_timeout - description: "If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more." + description: "If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: OFF field: general.vtol_transition_retry_on_airspeed_timeout type: bool - name: nav_vtol_transition_fail_action_mc_to_fw - description: "What iNAV should do if MC->FW transition still fails after the final attempt." + description: "What iNAV should do if MC->FW transition still fails after the final attempt. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: "IDLE" field: general.vtol_transition_fail_action_mc_to_fw table: nav_vtol_transition_fail_action_mc_to_fw - name: nav_vtol_transition_fail_action_fw_to_mc - description: "What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met." + description: "What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: "LOITER" field: general.vtol_transition_fail_action_fw_to_mc table: nav_vtol_transition_fail_action_fw_to_mc @@ -4015,37 +4024,43 @@ groups: min: 0 max: 100 - name: vtol_transition_to_fw_min_airspeed_cm_s - description: "Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted." + description: "Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: 0 field: vtolTransitionToFwMinAirspeed min: 0 max: 20000 - name: vtol_transition_to_mc_max_airspeed_cm_s - description: "When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted." + description: "When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: 0 field: vtolTransitionToMcMaxAirspeed min: 0 max: 20000 - name: vtol_fw_to_mc_auto_switch_airspeed_cm_s - description: "Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable." + description: "Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: 0 field: vtolFwToMcAutoSwitchAirspeed min: 0 max: 20000 - name: vtol_transition_lift_end_percent - description: "How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + description: "How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionLiftEndPercent min: 0 max: 100 - name: vtol_transition_mc_authority_end_percent - description: "How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + description: "How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionMcAuthorityEndPercent min: 0 max: 100 - name: vtol_transition_fw_authority_start_percent - description: "How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring in fixed-wing control more gently. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + description: "How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring it in and out more gently. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionFwAuthorityStartPercent min: 0 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 77038141281..10a96272c42 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -522,10 +522,16 @@ void FAST_CODE mixTable(void) input[PITCH] = axisPID[PITCH]; input[YAW] = axisPID[YAW]; if (isMixerTransitionMixing) { +#ifdef USE_AUTO_TRANSITION const float mcAuthorityScale = mixerATGetMcAuthorityScale(); input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f) * mcAuthorityScale; input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f) * mcAuthorityScale; input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f) * mcAuthorityScale; +#else + input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f); + input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f); + input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f); +#endif } } @@ -626,14 +632,14 @@ void FAST_CODE mixTable(void) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. - const float liftScale = isMixerTransitionMixing ? mixerATGetLiftScale() : 1.0f; - const float pusherScale = isMixerTransitionMixing ? mixerATGetPusherScale() : 1.0f; - for (int i = 0; i < motorCount; i++) { float motorThrottle = mixerThrottleCommand * currentMixer[i].throttle; +#ifdef USE_AUTO_TRANSITION + const float liftScale = isMixerTransitionMixing ? mixerATGetLiftScale() : 1.0f; if (currentMixer[i].throttle > 0.0f) { motorThrottle *= liftScale; } +#endif motor[i] = rpyMix[i] + constrain(motorThrottle, throttleMin, throttleMax); @@ -649,9 +655,14 @@ void FAST_CODE mixTable(void) } //spin stopped motors only in mixer transition mode if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && !feature(FEATURE_REVERSIBLE_MOTORS)) { +#ifdef USE_AUTO_TRANSITION + const float pusherScale = mixerATGetPusherScale(); const float pusherTarget = -currentMixer[i].throttle * 1000.0f; const float pusherIdle = throttleRangeMin; motor[i] = pusherIdle + (pusherTarget - pusherIdle) * pusherScale; +#else + motor[i] = -currentMixer[i].throttle * 1000; +#endif motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } } diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index bdd322e3301..67f2e025b56 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -40,10 +40,12 @@ bool isMixerTransitionMixing; bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; int nextMixerProfileIndex; +#ifdef USE_AUTO_TRANSITION static bool manualTransitionModeWasActive; static bool manualTransitionReadyForEdge = true; static bool manualTransitionSessionLatched; static bool manualFwToMcProtectionLatched; +#endif PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 4); @@ -61,10 +63,12 @@ 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, +#ifdef USE_AUTO_TRANSITION .vtolTransitionDynamicMixer = SETTING_MIXER_VTOL_TRANSITION_DYNAMIC_MIXER_DEFAULT, .manualVtolTransitionController = SETTING_MIXER_VTOL_MANUALSWITCH_AUTOTRANSITION_CONTROLLER_DEFAULT, .vtolTransitionAirspeedTimeoutMs = SETTING_MIXER_VTOL_TRANSITION_AIRSPEED_TIMEOUT_MS_DEFAULT, .vtolTransitionScaleRampTimeMs = SETTING_MIXER_VTOL_TRANSITION_SCALE_RAMP_TIME_MS_DEFAULT, +#endif .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, @@ -120,6 +124,7 @@ void mixerConfigInit(void) void setMixerProfileAT(void) { +#ifdef USE_AUTO_TRANSITION const timeMs_t now = millis(); mixerProfileAT.transitionStartTime = now; @@ -136,8 +141,13 @@ void setMixerProfileAT(void) mixerProfileAT.liftScale = 1.0f; mixerProfileAT.mcAuthorityScale = 1.0f; mixerProfileAT.fwAuthorityScale = 1.0f; +#else + mixerProfileAT.transitionStartTime = millis(); + mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; +#endif } +#ifdef USE_AUTO_TRANSITION static bool requestTransitionsToFixedWing(const mixerProfileATRequest_e required_action) { return required_action == MIXERAT_REQUEST_RTH || @@ -391,6 +401,7 @@ static bool mixerATReadyForHotSwitch(const mixerProfileATRequest_e required_acti return elapsedMs >= transitionTimerMs; } +#endif bool platformTypeConfigured(flyingPlatformType_e platformType) { @@ -400,6 +411,7 @@ bool platformTypeConfigured(flyingPlatformType_e platformType) return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType; } +#ifdef USE_AUTO_TRANSITION static bool missionTransitionToMultirotorTypeConfigured(void) { if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) { @@ -409,6 +421,7 @@ static bool missionTransitionToMultirotorTypeConfigured(void) const flyingPlatformType_e nextPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; return isMultirotorTypePlatform(nextPlatformType); } +#endif bool checkMixerATRequired(mixerProfileATRequest_e required_action) { @@ -422,6 +435,7 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) return false; } +#ifdef USE_AUTO_TRANSITION switch (required_action) { case MIXERAT_REQUEST_RTH: return currentMixerConfig.automated_switch && STATE(MULTIROTOR) && platformTypeConfigured(PLATFORM_AIRPLANE); @@ -444,14 +458,28 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) default: return false; } +#else + if(currentMixerConfig.automated_switch){ + if ((required_action == MIXERAT_REQUEST_RTH) && STATE(MULTIROTOR)) + { + return true; + } + if ((required_action == MIXERAT_REQUEST_LAND) && STATE(AIRPLANE)) + { + return true; + } + } + return false; +#endif } bool mixerATUpdateState(mixerProfileATRequest_e required_action) { +#ifdef USE_AUTO_TRANSITION //return true if mixerAT is done or not required bool reprocessState; do - { + { reprocessState=false; if (required_action == MIXERAT_REQUEST_ABORT) { abortTransition(false); @@ -512,6 +540,48 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) } while (reprocessState); return true; +#else + //return true if mixerAT is done or not required + bool reprocessState; + do + { + reprocessState=false; + if (required_action==MIXERAT_REQUEST_ABORT){ + isMixerTransitionMixing_requested = false; + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + return true; + } + switch (mixerProfileAT.phase){ + case MIXERAT_PHASE_IDLE: + //check if mixerAT is required + if (checkMixerATRequired(required_action)){ + mixerProfileAT.phase=MIXERAT_PHASE_TRANSITION_INITIALIZE; + reprocessState = true; + } + break; + case MIXERAT_PHASE_TRANSITION_INITIALIZE: + setMixerProfileAT(); + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; + reprocessState = true; + break; + case MIXERAT_PHASE_TRANSITIONING: + isMixerTransitionMixing_requested = true; + if (millis() > mixerProfileAT.transitionTransEndTime){ + isMixerTransitionMixing_requested = false; + outputProfileHotSwitch(nextMixerProfileIndex); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + reprocessState = true; + //transition is done + } + return false; + break; + default: + break; + } + } + while (reprocessState); + return true; +#endif } bool checkMixerProfileHotSwitchAvalibility(void) @@ -530,6 +600,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) return; } +#ifdef USE_AUTO_TRANSITION bool mixerAT_inuse = mixerATIsActive(); const bool transitionModeActive = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); const bool transitionModeRisingEdge = transitionModeActive && !manualTransitionModeWasActive; @@ -675,6 +746,18 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) if (!isMixerTransitionMixing) { resetTransitionScales(); } +#else + bool mixerAT_inuse = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; + // transition mode input for servo mix and motor mix + if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) + { + if (isModeActivationConditionPresent(BOXMIXERPROFILE)){ + outputProfileHotSwitch(IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1); + } + isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); + } + isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); +#endif } bool mixerATIsActive(void) @@ -684,37 +767,65 @@ bool mixerATIsActive(void) bool mixerATWasAborted(void) { +#ifdef USE_AUTO_TRANSITION return mixerProfileAT.aborted; +#else + return false; +#endif } bool mixerATWasAbortedByAirspeedTimeout(void) { +#ifdef USE_AUTO_TRANSITION return mixerProfileAT.abortedByAirspeedTimeout; +#else + return false; +#endif } float mixerATGetPusherScale(void) { +#ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif } float mixerATGetLiftScale(void) { +#ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif } float mixerATGetMcAuthorityScale(void) { +#ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif } float mixerATGetFwAuthorityScale(void) { +#ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif } float mixerATGetBlendToFw(void) { +#ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.blendToFw, 0.0f, 1.0f); +#else + return 1.0f; +#endif } bool isMixerProfile2ModeReportedActive(void) @@ -728,6 +839,7 @@ bool isMixerProfile2ModeReportedActive(void) bool isMixerTransitionModeReportedActive(void) { +#ifdef USE_AUTO_TRANSITION // Transition is actively running in the internal controller. if (mixerATIsActive()) { return true; @@ -739,6 +851,9 @@ bool isMixerTransitionModeReportedActive(void) } return IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); +#else + return IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); +#endif } // switch mixerprofile without reboot diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index cbc69066ea0..05a4a9d1bc8 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,10 +18,12 @@ typedef struct mixerConfig_s { bool controlProfileLinking; bool automated_switch; int16_t switchTransitionTimer; +#ifdef USE_AUTO_TRANSITION bool vtolTransitionDynamicMixer; bool manualVtolTransitionController; uint16_t vtolTransitionAirspeedTimeoutMs; uint16_t vtolTransitionScaleRampTimeMs; +#endif bool tailsitterOrientationOffset; int16_t transition_PID_mmix_multiplier_roll; int16_t transition_PID_mmix_multiplier_pitch; @@ -38,28 +40,36 @@ typedef enum { MIXERAT_REQUEST_NONE, //no request, stats checking only MIXERAT_REQUEST_RTH, MIXERAT_REQUEST_LAND, +#ifdef USE_AUTO_TRANSITION MIXERAT_REQUEST_MISSION_TO_FW, MIXERAT_REQUEST_MISSION_TO_MC, MIXERAT_REQUEST_MANUAL_TO_FW, MIXERAT_REQUEST_MANUAL_TO_MC, +#endif MIXERAT_REQUEST_ABORT, } mixerProfileATRequest_e; +#ifdef USE_AUTO_TRANSITION typedef enum { MIXERAT_DIRECTION_NONE = 0, MIXERAT_DIRECTION_TO_FW, MIXERAT_DIRECTION_TO_MC, } mixerProfileATDirection_e; +#endif //mixerProfile Automated Transition PHASE typedef enum { MIXERAT_PHASE_IDLE, MIXERAT_PHASE_TRANSITION_INITIALIZE, MIXERAT_PHASE_TRANSITIONING, +#ifndef USE_AUTO_TRANSITION + MIXERAT_PHASE_DONE, +#endif } mixerProfileATState_e; typedef struct mixerProfileAT_s { mixerProfileATState_e phase; +#ifdef USE_AUTO_TRANSITION mixerProfileATDirection_e direction; mixerProfileATRequest_e request; bool aborted; @@ -76,6 +86,12 @@ typedef struct mixerProfileAT_s { float mcAuthorityScale; float fwAuthorityScale; timeMs_t transitionStartTime; +#else + bool transitionInputMixing; + timeMs_t transitionStartTime; + timeMs_t transitionStabEndTime; + timeMs_t transitionTransEndTime; +#endif } mixerProfileAT_t; extern mixerProfileAT_t mixerProfileAT; bool checkMixerATRequired(mixerProfileATRequest_e required_action); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 58fa056e14f..f36f6912653 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -119,6 +119,33 @@ typedef struct { fwPidAttenuation_t attenuation; } pidState_t; +#ifdef USE_AUTO_TRANSITION +typedef struct { + float kP; + float kI; + float kD; + float kFF; + float gyroRate; + float rateTarget; + float errorGyroIf; + float errorGyroIfLimit; + pt1Filter_t ptermLpfState; + filter_t dtermLpfState; + float previousRateTarget; + float previousRateGyro; +#ifdef USE_D_BOOST + pt1Filter_t dBoostLpf; + biquadFilter_t dBoostGyroLpf; + float dBoostTargetAcceleration; +#endif + filterApply4FnPtr ptermFilterApplyFn; +#ifdef USE_SMITH_PREDICTOR + smithPredictor_t smithPredictor; +#endif + fwPidAttenuation_t attenuation; +} autoTransitionTargetPidState_t; +#endif + STATIC_FASTRAM bool pidFiltersConfigured = false; static EXTENDED_FASTRAM float headingHoldCosZLimit; static EXTENDED_FASTRAM int16_t headingHoldTarget; @@ -128,6 +155,9 @@ static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter; // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied STATIC_FASTRAM bool pidGainsUpdateRequired= true; FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; +#ifdef USE_AUTO_TRANSITION +FASTRAM int16_t autoTransitionTargetAxisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; +#endif #ifdef USE_BLACKBOX int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT]; @@ -138,8 +168,17 @@ int32_t axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT]; #endif static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT]; +#ifdef USE_AUTO_TRANSITION +static EXTENDED_FASTRAM autoTransitionTargetPidState_t autoTransitionTargetPidState[FLIGHT_DYNAMICS_INDEX_COUNT]; +#endif static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; static EXTENDED_FASTRAM uint8_t itermRelax; +#ifdef USE_AUTO_TRANSITION +static EXTENDED_FASTRAM pt1Filter_t autoTransitionTargetTpaFilter; +static EXTENDED_FASTRAM filterApplyFnPtr autoTransitionTargetDTermLpfFilterApplyFn; +static EXTENDED_FASTRAM uint8_t autoTransitionTargetYawLpfHz; +static EXTENDED_FASTRAM int8_t autoTransitionTargetControlProfileIndex = -1; +#endif #ifdef USE_ANTIGRAVITY static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf; @@ -180,6 +219,8 @@ static EXTENDED_FASTRAM timeMs_t pidLoopNowMs; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; +static void applyItermLimiting(pidState_t *pidState); + PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, @@ -395,6 +436,11 @@ void pidResetErrorAccumulators(void) for (int axis = 0; axis < 3; axis++) { pidState[axis].errorGyroIf = 0.0f; pidState[axis].errorGyroIfLimit = 0.0f; +#ifdef USE_AUTO_TRANSITION + autoTransitionTargetPidState[axis].errorGyroIf = 0.0f; + autoTransitionTargetPidState[axis].errorGyroIfLimit = 0.0f; + autoTransitionTargetAxisPID[axis] = 0; +#endif } } @@ -414,6 +460,372 @@ float getAxisIterm(uint8_t axis) return pidState[axis].errorGyroIf; } +#ifdef USE_AUTO_TRANSITION +static void resetAutoTransitionTargetPidState(void) +{ + memset(autoTransitionTargetPidState, 0, sizeof(autoTransitionTargetPidState)); + memset(autoTransitionTargetAxisPID, 0, sizeof(autoTransitionTargetAxisPID)); + memset(&autoTransitionTargetTpaFilter, 0, sizeof(autoTransitionTargetTpaFilter)); + autoTransitionTargetControlProfileIndex = -1; + autoTransitionTargetDTermLpfFilterApplyFn = (filterApplyFnPtr)nullFilterApply; + autoTransitionTargetYawLpfHz = 0; +} + +static bool isAutoTransitionTargetPidActive(void) +{ + return isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && + isMultirotorTypePlatform(currentMixerConfig.platformType) && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_PROFILE_COUNT && + mixerConfigByIndex(nextMixerProfileIndex)->platformType == PLATFORM_AIRPLANE; +} + +static uint8_t getAutoTransitionTargetControlProfileIndex(void) +{ + if (currentMixerConfig.controlProfileLinking && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_CONTROL_PROFILE_COUNT && + nextMixerProfileIndex < MAX_PROFILE_COUNT) { + return nextMixerProfileIndex; + } + + return getConfigProfile(); +} + +static const controlConfig_t *getAutoTransitionTargetControlProfile(uint8_t profileIndex) +{ + if (currentMixerConfig.controlProfileLinking && + profileIndex < MAX_CONTROL_PROFILE_COUNT) { + return controlProfiles(profileIndex); + } + + return currentControlProfile; +} + +static const pidProfile_t *getAutoTransitionTargetPidProfile(uint8_t profileIndex) +{ + if (currentMixerConfig.controlProfileLinking && profileIndex < MAX_PROFILE_COUNT) { + const pgRegistry_t *pidProfileRegistry = pgFind(PG_PID_PROFILE); + if (pidProfileRegistry) { + return (const pidProfile_t *)(pidProfileRegistry->address + (pgSize(pidProfileRegistry) * profileIndex)); + } + } + + return pidProfile(); +} + +static void initAutoTransitionTargetPidState(const uint8_t profileIndex, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile) +{ + const uint32_t refreshRate = getLooptime(); + + resetAutoTransitionTargetPidState(); + + autoTransitionTargetControlProfileIndex = profileIndex; + autoTransitionTargetYawLpfHz = targetPidProfile->yaw_lpf_hz; + + assignFilterApplyFn(targetPidProfile->dterm_lpf_type, targetPidProfile->dterm_lpf_hz, &autoTransitionTargetDTermLpfFilterApplyFn); + + if (controlProfile->throttle.fixedWingTauMs > 0) { + pt1FilterInitRC(&autoTransitionTargetTpaFilter, MS2S(controlProfile->throttle.fixedWingTauMs), US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ))); + pt1FilterReset(&autoTransitionTargetTpaFilter, getThrottleIdleValue()); + } + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { +#ifdef USE_D_BOOST + autoTransitionTargetPidState[axis].dBoostTargetAcceleration = controlProfile->stabilized.rates[axis] * 10 * 10; +#endif + + if (axis == FD_YAW && autoTransitionTargetYawLpfHz) { + autoTransitionTargetPidState[axis].ptermFilterApplyFn = (filterApply4FnPtr)pt1FilterApply4; + } else { + autoTransitionTargetPidState[axis].ptermFilterApplyFn = (filterApply4FnPtr)nullFilterApply4; + } + + initFilter(targetPidProfile->dterm_lpf_type, &autoTransitionTargetPidState[axis].dtermLpfState, targetPidProfile->dterm_lpf_hz, refreshRate); + +#ifdef USE_D_BOOST + biquadFilterInitLPF(&autoTransitionTargetPidState[axis].dBoostGyroLpf, targetPidProfile->dBoostGyroDeltaLpfHz, refreshRate); +#endif + +#ifdef USE_SMITH_PREDICTOR + smithPredictorInit( + &autoTransitionTargetPidState[axis].smithPredictor, + targetPidProfile->smithPredictorDelay, + targetPidProfile->smithPredictorStrength, + targetPidProfile->smithPredictorFilterHz, + refreshRate + ); +#endif + } +} + +static float calculateAutoTransitionTargetAirspeedTPAFactor(const pidProfile_t *targetPidProfile, const controlConfig_t *controlProfile) +{ + const float airspeed = constrainf(getAirspeedEstimate(), 100.0f, 20000.0f); + const float referenceAirspeed = targetPidProfile->fixedWingReferenceAirspeed; + float tpaFactor = powf(referenceAirspeed / airspeed, controlProfile->throttle.apa_pow / 100.0f); + + return constrainf(tpaFactor, 0.3f, 2.0f); +} + +static float calculateAutoTransitionTargetAirspeedITermFactor(const pidProfile_t *targetPidProfile, const controlConfig_t *controlProfile) +{ + const float airspeed = constrainf(getAirspeedEstimate(), 100.0f, 20000.0f); + const float referenceAirspeed = targetPidProfile->fixedWingReferenceAirspeed; + const float apaPow = controlProfile->throttle.apa_pow; + + if (apaPow <= 100.0f) { + return 1.0f; + } + + return constrainf(powf(referenceAirspeed / airspeed, (apaPow / 100.0f) - 1.0f), 0.3f, 1.5f); +} + +static uint16_t calculateAutoTransitionTargetTPAThrottle(const controlConfig_t *controlProfile) +{ + if (controlProfile->throttle.fixedWingTauMs > 0) { + static const fpVector3_t vDown = { .v = { 0.0f, 0.0f, 1.0f } }; + fpVector3_t vForward = { .v = { HeadVecEFFiltered.x, -HeadVecEFFiltered.y, -HeadVecEFFiltered.z } }; + const float groundCos = vectorDotProduct(&vForward, &vDown); + const int16_t throttleAdjustment = controlProfile->throttle.tpa_pitch_compensation * groundCos * 90.0f / 1.57079632679f; + const uint16_t throttleAdjusted = rcCommand[THROTTLE] + constrain(throttleAdjustment, -1000, 1000); + return pt1FilterApply(&autoTransitionTargetTpaFilter, constrain(throttleAdjusted, 1000, 2000)); + } + + return rcCommand[THROTTLE]; +} + +static float calculateAutoTransitionTargetTPAFactor(const controlConfig_t *controlProfile) +{ + const uint16_t throttle = calculateAutoTransitionTargetTPAThrottle(controlProfile); + float tpaFactor; + + if (controlProfile->throttle.dynPID != 0 && + controlProfile->throttle.pa_breakpoint > getThrottleIdleValue() && + !FLIGHT_MODE(AUTO_TUNE) && + ARMING_FLAG(ARMED)) { + if (throttle > getThrottleIdleValue()) { + tpaFactor = 0.5f + ((float)(controlProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); + } else { + tpaFactor = 2.0f; + } + + tpaFactor = 1.0f + (tpaFactor - 1.0f) * (controlProfile->throttle.dynPID / 100.0f); + tpaFactor = constrainf(tpaFactor, 0.3f, 2.0f); + } else { + tpaFactor = 1.0f; + } + + return tpaFactor; +} + +static void updateAutoTransitionTargetPIDCoefficients(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile) +{ + float tpaFactor = 1.0f; + float iTermFactor = 1.0f; + + if (controlProfile->throttle.apa_pow > 0 && + pitotValidForAirspeed()) { + tpaFactor = calculateAutoTransitionTargetAirspeedTPAFactor(targetPidProfile, controlProfile); + iTermFactor = calculateAutoTransitionTargetAirspeedITermFactor(targetPidProfile, controlProfile); + } else { + tpaFactor = calculateAutoTransitionTargetTPAFactor(controlProfile); + iTermFactor = tpaFactor; + } + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + autoTransitionTargetPidState[axis].kP = targetPidProfile->bank_fw.pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; + autoTransitionTargetPidState[axis].kI = targetPidProfile->bank_fw.pid[axis].I / FP_PID_RATE_I_MULTIPLIER * iTermFactor; + autoTransitionTargetPidState[axis].kD = targetPidProfile->bank_fw.pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor; + autoTransitionTargetPidState[axis].kFF = targetPidProfile->bank_fw.pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; + } +} + +static float autoTransitionTargetPTermProcess(autoTransitionTargetPidState_t *pidState, flight_dynamics_index_t axis, float rateError, float dT) +{ + const float newPTerm = rateError * pidState->kP; + UNUSED(axis); + + return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, autoTransitionTargetYawLpfHz, dT); +} + +#ifdef USE_D_BOOST +static float applyAutoTransitionTargetDBoost(autoTransitionTargetPidState_t *pidState, const pidProfile_t *targetPidProfile, float currentRateTarget, float dT, float dT_inv) +{ + float dBoost = 1.0f; + const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv; + const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); + const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv); + + if (dBoostGyroAcceleration >= dBoostRateAcceleration) { + dBoost = scaleRangef(dBoostGyroAcceleration, 0.0f, targetPidProfile->dBoostMaxAtAlleceleration, 1.0f, targetPidProfile->dBoostMax); + } else { + dBoost = scaleRangef(dBoostRateAcceleration, 0.0f, pidState->dBoostTargetAcceleration, 1.0f, targetPidProfile->dBoostMin); + } + + dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); + return constrainf(dBoost, targetPidProfile->dBoostMin, targetPidProfile->dBoostMax); +} +#else +static float applyAutoTransitionTargetDBoost(autoTransitionTargetPidState_t *pidState, const pidProfile_t *targetPidProfile, float currentRateTarget, float dT, float dT_inv) +{ + UNUSED(pidState); + UNUSED(targetPidProfile); + UNUSED(currentRateTarget); + UNUSED(dT); + UNUSED(dT_inv); + return 1.0f; +} +#endif + +static float autoTransitionTargetDTermProcess(autoTransitionTargetPidState_t *pidState, const pidProfile_t *targetPidProfile, float currentRateTarget, float dT, float dT_inv) +{ + if (pidState->kD == 0.0f) { + return 0.0f; + } + + float delta = pidState->previousRateGyro - pidState->gyroRate; + delta = autoTransitionTargetDTermLpfFilterApplyFn((filter_t *)&pidState->dtermLpfState, delta); + + return delta * (pidState->kD * dT_inv) * applyAutoTransitionTargetDBoost(pidState, targetPidProfile, currentRateTarget, dT, dT_inv); +} + +static void applyAutoTransitionTargetItermLock(autoTransitionTargetPidState_t *pidState, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis, const float rateTarget, const float rateError) +{ + const float maxRate = controlProfile->stabilized.rates[axis] * 10.0f; + const float dampingFactor = attenuation(rateTarget, maxRate * targetPidProfile->fwItermLockRateLimit / 100.0f); + const bool errorThresholdReached = fabsf(rateError) > maxRate * targetPidProfile->fwItermLockEngageThreshold / 100.0f; + + if (fabsf(rateTarget) > maxRate * 0.2f) { + pidState->attenuation.targetOverThresholdTimeMs = pidLoopNowMs; + } + + if (!errorThresholdReached) { + pidState->attenuation.targetOverThresholdTimeMs = 0; + } + + pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (pidLoopNowMs - pidState->attenuation.targetOverThresholdTimeMs) < targetPidProfile->fwItermLockTimeMaxMs) ? 0.0f : 1.0f); + pidState->attenuation.aP = dampingFactor; + pidState->attenuation.aD = dampingFactor; +} + +static bool checkAutoTransitionTargetItermLimitingActive(void) +{ + return STATE(ANTI_WINDUP); +} + +static void applyAutoTransitionTargetItermLimiting(autoTransitionTargetPidState_t *pidState, const bool itermLimitActive) +{ + if (itermLimitActive) { + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); + } else { + pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); + } +} + +static bool checkAutoTransitionTargetItermFreezingActive(const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis) +{ + if (targetPidProfile->fixedWingYawItermBankFreeze != 0 && + axis == FD_YAW) { + const float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); + return fabsf(bankAngle) > targetPidProfile->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT)); + } + + return false; +} + +static int16_t applyAutoTransitionTargetFixedWingRateController(autoTransitionTargetPidState_t *pidState, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis, const bool itermLimitActive, const bool itermFreezeActive, float dT, float dT_inv) +{ + const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); + const float rateError = rateTarget - pidState->gyroRate; + + applyAutoTransitionTargetItermLock(pidState, controlProfile, targetPidProfile, axis, rateTarget, rateError); + + const float newPTerm = autoTransitionTargetPTermProcess(pidState, axis, rateError, dT) * pidState->attenuation.aP; + const float newDTerm = autoTransitionTargetDTermProcess(pidState, targetPidProfile, rateTarget, dT, dT_inv) * pidState->attenuation.aD; + const float newFFTerm = rateTarget * pidState->kFF; + + if (!itermFreezeActive) { + pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI; + } + + applyAutoTransitionTargetItermLimiting(pidState, itermLimitActive); + + const uint16_t limit = 500; + + if (targetPidProfile->pidItermLimitPercent != 0) { + const float itermLimit = limit * targetPidProfile->pidItermLimitPercent * 0.01f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); + } + + pidState->previousRateGyro = pidState->gyroRate; + pidState->previousRateTarget = rateTarget; + + return constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -limit, +limit); +} + +static void updateAutoTransitionTargetAxisPID(float dT) +{ + if (!pidFiltersConfigured || !isAutoTransitionTargetPidActive()) { + resetAutoTransitionTargetPidState(); + return; + } + + const uint8_t profileIndex = getAutoTransitionTargetControlProfileIndex(); + const controlConfig_t *controlProfile = getAutoTransitionTargetControlProfile(profileIndex); + const pidProfile_t *targetPidProfile = getAutoTransitionTargetPidProfile(profileIndex); + + if (autoTransitionTargetControlProfileIndex != profileIndex) { + initAutoTransitionTargetPidState(profileIndex, controlProfile, targetPidProfile); + } + + updateAutoTransitionTargetPIDCoefficients(controlProfile, targetPidProfile); + + const float dT_inv = 1.0f / dT; + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + autoTransitionTargetPidState[axis].gyroRate = gyro.gyroADCf[axis]; + autoTransitionTargetPidState[axis].rateTarget = pidState[axis].rateTarget; + +#ifdef USE_SMITH_PREDICTOR + autoTransitionTargetPidState[axis].gyroRate = applySmithPredictor(axis, &autoTransitionTargetPidState[axis].smithPredictor, autoTransitionTargetPidState[axis].gyroRate); +#endif + + const bool itermLimitActive = checkAutoTransitionTargetItermLimitingActive(); + const bool itermFreezeActive = checkAutoTransitionTargetItermFreezingActive(targetPidProfile, axis); + + autoTransitionTargetAxisPID[axis] = applyAutoTransitionTargetFixedWingRateController(&autoTransitionTargetPidState[axis], controlProfile, targetPidProfile, axis, itermLimitActive, itermFreezeActive, dT, dT_inv); + } +} + +int16_t getAutoTransitionTargetStabilizedInput(flight_dynamics_index_t axis) +{ + if (!(isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction != MIXERAT_DIRECTION_NONE)) { + return 0; + } + + const float scale = currentMixerConfig.vtolTransitionDynamicMixer ? mixerATGetFwAuthorityScale() : 1.0f; + + if (FLIGHT_MODE(MANUAL_MODE)) { + return lrintf(rcCommand[axis] * scale); + } + + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + return lrintf(autoTransitionTargetAxisPID[axis] * scale); + } + + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + return lrintf(axisPID[axis] * scale); + } + + return 0; +} +#endif + static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination) { stick = constrain(stick, -500, 500); @@ -1226,6 +1638,9 @@ void FAST_CODE pidController(float dT) pidLoopNowMs = millis(); if (!pidFiltersConfigured) { +#ifdef USE_AUTO_TRANSITION + resetAutoTransitionTargetPidState(); +#endif return; } @@ -1326,6 +1741,10 @@ void FAST_CODE pidController(float dT) pidControllerApplyFn(&pidState[axis], dT, dT_inv); } + +#ifdef USE_AUTO_TRANSITION + updateAutoTransitionTargetAxisPID(dT); +#endif } pidType_e pidIndexGetType(pidIndex_e pidIndex) @@ -1427,6 +1846,10 @@ void pidInit(void) 0.0f ); +#ifdef USE_AUTO_TRANSITION + resetAutoTransitionTargetPidState(); +#endif + } const pidBank_t * pidBank(void) { @@ -1508,4 +1931,4 @@ uint16_t getPidSumLimit(const flight_dynamics_index_t axis) { } else { return 500; } -} \ No newline at end of file +} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ff2e85031b7..5c613d37a16 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -187,7 +187,13 @@ const pidBank_t * pidBank(void); pidBank_t * pidBankMutable(void); extern int16_t axisPID[]; +#ifdef USE_AUTO_TRANSITION +extern int16_t autoTransitionTargetAxisPID[]; +#endif extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_F[], axisPID_Setpoint[]; +#ifdef USE_AUTO_TRANSITION +int16_t getAutoTransitionTargetStabilizedInput(flight_dynamics_index_t axis); +#endif void pidInit(void); bool pidInitFilters(void); @@ -227,4 +233,4 @@ bool isFixedWingLevelTrimActive(void); void updateFixedWingLevelTrim(timeUs_t currentTimeUs); float getFixedWingLevelTrim(void); bool isAngleHoldLevel(void); -uint16_t getPidSumLimit(const flight_dynamics_index_t axis); \ No newline at end of file +uint16_t getPidSumLimit(const flight_dynamics_index_t axis); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 8c9276a1fbc..b3b7e11a3b9 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -277,6 +277,79 @@ static void filterServos(void) } } +#ifdef USE_AUTO_TRANSITION +static bool isAutoTransitionTargetInputSource(const uint8_t inputSource) +{ + return inputSource >= INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL && + inputSource <= INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS; +} + +static bool isStabilizedAxisInputSource(const uint8_t inputSource) +{ + switch (inputSource) { + case INPUT_STABILIZED_ROLL: + case INPUT_STABILIZED_PITCH: + case INPUT_STABILIZED_YAW: + case INPUT_STABILIZED_ROLL_PLUS: + case INPUT_STABILIZED_ROLL_MINUS: + case INPUT_STABILIZED_PITCH_PLUS: + case INPUT_STABILIZED_PITCH_MINUS: + case INPUT_STABILIZED_YAW_PLUS: + case INPUT_STABILIZED_YAW_MINUS: + return true; + default: + return false; + } +} + +static int getAutoTransitionServoProfileIndex(void) +{ + if (isMultirotorTypePlatform(currentMixerConfig.platformType)) { + return currentMixerProfileIndex; + } + + if (nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT && + isMultirotorTypePlatform(mixerConfigByIndex(nextMixerProfileIndex)->platformType)) { + return nextMixerProfileIndex; + } + + return -1; +} + +static void collectAutoTransitionServoTargets(bool targets[MAX_SUPPORTED_SERVOS]) +{ + memset(targets, 0, MAX_SUPPORTED_SERVOS * sizeof(targets[0])); + + const int profileIndex = getAutoTransitionServoProfileIndex(); + if (profileIndex < 0) { + return; + } + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + const servoMixer_t *rule = &mixerServoMixersByIndex(profileIndex)[i]; + + if (rule->rate == 0) { + break; + } + + if (!isAutoTransitionTargetInputSource(rule->inputSource)) { + continue; + } + +#ifdef USE_PROGRAMMING_FRAMEWORK + if (!logicConditionGetValue(rule->conditionId)) { + continue; + } +#endif + + if (rule->targetChannel < MAX_SUPPORTED_SERVOS) { + targets[rule->targetChannel] = true; + } + } +} +#endif + void writeServos(void) { filterServos(); @@ -305,6 +378,22 @@ void writeServos(void) void servoMixer(float dT) { int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] +#ifdef USE_AUTO_TRANSITION + const bool autoTransitionInputsActive = isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction != MIXERAT_DIRECTION_NONE; + const bool scaleCurrentFwServoRules = autoTransitionInputsActive && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && + !isMultirotorTypePlatform(currentMixerConfig.platformType); + const float currentFwServoScale = currentMixerConfig.vtolTransitionDynamicMixer ? mixerATGetFwAuthorityScale() : 1.0f; + bool autoTransitionServoTargets[MAX_SUPPORTED_SERVOS]; + + if (scaleCurrentFwServoRules) { + collectAutoTransitionServoTargets(autoTransitionServoTargets); + } else { + memset(autoTransitionServoTargets, 0, sizeof(autoTransitionServoTargets)); + } +#endif if (FLIGHT_MODE(MANUAL_MODE)) { input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; @@ -323,12 +412,26 @@ void servoMixer(float dT) } } +#ifdef USE_AUTO_TRANSITION + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL] = getAutoTransitionTargetStabilizedInput(FD_ROLL); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH] = getAutoTransitionTargetStabilizedInput(FD_PITCH); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW] = getAutoTransitionTargetStabilizedInput(FD_YAW); +#endif + input[INPUT_STABILIZED_ROLL_PLUS] = constrain(input[INPUT_STABILIZED_ROLL], 0, 1000); input[INPUT_STABILIZED_ROLL_MINUS] = constrain(input[INPUT_STABILIZED_ROLL], -1000, 0); input[INPUT_STABILIZED_PITCH_PLUS] = constrain(input[INPUT_STABILIZED_PITCH], 0, 1000); input[INPUT_STABILIZED_PITCH_MINUS] = constrain(input[INPUT_STABILIZED_PITCH], -1000, 0); input[INPUT_STABILIZED_YAW_PLUS] = constrain(input[INPUT_STABILIZED_YAW], 0, 1000); input[INPUT_STABILIZED_YAW_MINUS] = constrain(input[INPUT_STABILIZED_YAW], -1000, 0); +#ifdef USE_AUTO_TRANSITION + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_PLUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL], 0, 1000); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_MINUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL], -1000, 0); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_PLUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH], 0, 1000); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_MINUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH], -1000, 0); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_PLUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW], 0, 1000); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW], -1000, 0); +#endif input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0; @@ -455,6 +558,15 @@ void servoMixer(float dT) inputRaw = 0; } #endif + +#ifdef USE_AUTO_TRANSITION + if (scaleCurrentFwServoRules && + autoTransitionServoTargets[target] && + isStabilizedAxisInputSource(from)) { + inputRaw = lrintf(inputRaw * currentFwServoScale); + } +#endif + /* * Apply mixer speed limit. 1 [one] speed unit is defined as 10us/s: * 0 = no limiting diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index b16dd7ca915..593f5774fe5 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -85,6 +85,17 @@ typedef enum { INPUT_RC_CH33 = 58, INPUT_RC_CH34 = 59, INPUT_MIXER_SWITCH_HELPER = 60, +#ifdef USE_AUTO_TRANSITION + INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL = 61, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH = 62, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW = 63, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_PLUS = 64, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_MINUS = 65, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_PLUS = 66, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_MINUS = 67, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_PLUS = 68, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS = 69, +#endif INPUT_SOURCE_COUNT } inputSource_e; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 7d7120dd85c..21bfdd4875f 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -84,6 +84,7 @@ #define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec) #define FW_LAND_LOITER_ALT_TOLERANCE 150 +#ifdef USE_AUTO_TRANSITION // One-shot MC->FW mission retry after airspeed-timeout: hold position, yaw scan, align to best pitot heading. #define NAV_MIXERAT_RETRY_SCAN_STEP_CD DEGREES_TO_CENTIDEGREES(20) #define NAV_MIXERAT_RETRY_HEADING_TOL_CD DEGREES_TO_CENTIDEGREES(5) @@ -91,6 +92,7 @@ #define NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS 6000 #define NAV_MIXERAT_RETRY_MAX_TOTAL_MS 45000 #define NAV_MIXERAT_MISSION_TRANSITION_TRACK_DISTANCE_CM 4000 +#endif /*----------------------------------------------------------- * Compatibility for home position @@ -128,7 +130,11 @@ 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 +#ifdef USE_AUTO_TRANSITION PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10); +#else +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7); +#endif PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -158,11 +164,13 @@ 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 +#ifdef USE_AUTO_TRANSITION .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_transition_retry_on_airspeed_timeout = SETTING_NAV_VTOL_TRANSITION_RETRY_ON_AIRSPEED_TIMEOUT_DEFAULT, .vtol_transition_fail_action_mc_to_fw = SETTING_NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_DEFAULT, .vtol_transition_fail_action_fw_to_mc = SETTING_NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_DEFAULT, +#endif #ifdef USE_MULTI_MISSION .waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT, // mission index selected from multi mission WP entry #endif @@ -284,6 +292,7 @@ uint16_t navEPV; int16_t navAccNEU[3]; //End of blackbox states +#ifdef USE_AUTO_TRANSITION typedef struct navMixerATMissionTransition_s { mixerProfileATRequest_e request; int32_t heading; @@ -323,6 +332,9 @@ typedef enum { static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; static navMixerATMissionTransition_t navMixerATMissionTransition; +#else +static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; +#endif static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode); static void updateDesiredRTHAltitude(void); @@ -342,7 +354,9 @@ static void resetJumpCounter(void); static void clearJumpCounters(void); static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); +#ifdef USE_AUTO_TRANSITION static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos); +#endif 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); @@ -350,6 +364,7 @@ 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); +#ifdef USE_AUTO_TRANSITION static void clearMissionVTOLTransitionState(void); static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const navWaypoint_t *waypoint); static void updateMissionTransitionGuidance(void); @@ -358,6 +373,7 @@ static bool hasAirspeedSensorForTransitionRetry(void); static bool canRetryTransitionAfterAirspeedTimeout(const mixerProfileATRequest_e request); static void beginMissionTransitionRetryScan(const mixerProfileATRequest_e request); static navMixerATRetryScanResult_e updateMissionTransitionRetryScan(void); +#endif void missionPlannerSetWaypoint(void); void initializeRTHSanityChecker(void); @@ -1107,12 +1123,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT, +#ifdef USE_AUTO_TRANSITION [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, +#endif [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 +#ifdef USE_AUTO_TRANSITION [NAV_FSM_EVENT_MIXERAT_MISSION_RESUME] = NAV_STATE_WAYPOINT_IN_PROGRESS, +#endif } }, [NAV_STATE_MIXERAT_ABORT] = { @@ -1326,6 +1346,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) { navigationFSMStateFlags_t stateFlags = navFSM[state].stateFlags; +#ifdef USE_AUTO_TRANSITION const bool mixerATState = (state == NAV_STATE_MIXERAT_INITIALIZE || state == NAV_STATE_MIXERAT_IN_PROGRESS); // During mission-authorized MC->FW transition, enable XY/YAW control to fly a straight acceleration segment. @@ -1342,6 +1363,7 @@ static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) isTransitionRetryToFixedWingRequest(navMixerATMissionTransition.request)) { stateFlags |= NAV_CTL_POS | NAV_CTL_YAW; } +#endif return stateFlags; } @@ -1367,8 +1389,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState { UNUSED(previousState); +#ifdef USE_AUTO_TRANSITION navMixerATPendingState = NAV_STATE_IDLE; clearMissionVTOLTransitionState(); +#endif resetAltitudeController(false); resetHeadingController(); resetPositionController(); @@ -2052,6 +2076,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void) } } +#ifdef USE_AUTO_TRANSITION static uint16_t missionUserActionMask(const navMissionUserAction_e userAction) { switch (userAction) { @@ -2387,6 +2412,7 @@ static void updateMissionTransitionGuidance(void) setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); } +#endif static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState) { @@ -2403,6 +2429,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; posControl.wpAltitudeReached = false; +#ifdef USE_AUTO_TRANSITION clearMissionVTOLTransitionState(); const navMissionVtolTransitionDisposition_e transitionAction = prepareMissionVTOLTransition(activeWaypoint); if (transitionAction == NAV_MISSION_VTOL_TRANSITION_START) { @@ -2411,6 +2438,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav if (transitionAction == NAV_MISSION_VTOL_TRANSITION_REJECT) { return NAV_FSM_EVENT_ERROR; } +#endif return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS } @@ -2729,11 +2757,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navi setupAltitudeController(); } +#ifdef USE_AUTO_TRANSITION if (previousState != NAV_STATE_WAYPOINT_PRE_ACTION) { clearMissionVTOLTransitionState(); } updateMissionTransitionGuidance(); +#else + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); +#endif navMixerATPendingState = previousState; return NAV_FSM_EVENT_SUCCESS; } @@ -2742,6 +2774,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav { UNUSED(previousState); +#ifdef USE_AUTO_TRANSITION if (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE)) { mixerATUpdateState(MIXERAT_REQUEST_ABORT); clearMissionVTOLTransitionState(); @@ -2841,13 +2874,54 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav updateMissionTransitionGuidance(); return NAV_FSM_EVENT_NONE; +#else + mixerProfileATRequest_e required_action; + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + required_action = MIXERAT_REQUEST_RTH; + break; + case NAV_STATE_RTH_LANDING: + required_action = MIXERAT_REQUEST_LAND; + break; + default: + required_action = MIXERAT_REQUEST_NONE; + break; + } + if (mixerATUpdateState(required_action)){ + // MixerAT is done, switch to next state + resetPositionController(); + resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + setupAltitudeController(); + return NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LANDING: + setupAltitudeController(); + return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + break; + default: + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + break; + } + } + + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + + return NAV_FSM_EVENT_NONE; +#endif } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState) { UNUSED(previousState); mixerATUpdateState(MIXERAT_REQUEST_ABORT); +#ifdef USE_AUTO_TRANSITION clearMissionVTOLTransitionState(); +#endif return NAV_FSM_EVENT_SUCCESS; } @@ -5565,8 +5639,10 @@ void navigationInit(void) { /* Initial state */ posControl.navState = NAV_STATE_IDLE; +#ifdef USE_AUTO_TRANSITION navMixerATPendingState = NAV_STATE_IDLE; clearMissionVTOLTransitionState(); +#endif posControl.flags.horizontalPositionDataNew = false; posControl.flags.verticalPositionDataNew = false; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index bff60d96d0e..42f7c7737f0 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -328,6 +328,7 @@ typedef enum { WP_MISSION_SWITCH, } navMissionRestart_e; +#ifdef USE_AUTO_TRANSITION typedef enum { NAV_MISSION_USER_ACTION_OFF = 0, NAV_MISSION_USER_ACTION_1, @@ -350,6 +351,7 @@ typedef enum { NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_EMERGENCY_LANDING, NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_FORCE_SWITCH, } navVtolTransitionFailActionFwToMc_e; +#endif typedef enum { RTH_TRACKBACK_OFF, @@ -436,11 +438,13 @@ 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 +#ifdef USE_AUTO_TRANSITION 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) bool vtol_transition_retry_on_airspeed_timeout; // Enables one-shot yaw-scan retry for failed airspeed-gated MC->FW auto-transition uint8_t vtol_transition_fail_action_mc_to_fw; // Action after final MC->FW transition failure uint8_t vtol_transition_fail_action_fw_to_mc; // Action after final FW->MC transition failure +#endif #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 947bc3406ea..aec088cf02a 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -183,7 +183,9 @@ 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, +#ifdef USE_AUTO_TRANSITION NAV_FSM_EVENT_MIXERAT_MISSION_RESUME = NAV_FSM_EVENT_STATE_SPECIFIC_4, +#endif NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5, NAV_FSM_EVENT_COUNT, diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 801239f4851..81f6125575d 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -687,6 +687,13 @@ static int logicConditionGetWaypointOperandValue(int operand) { } } +#ifdef USE_AUTO_TRANSITION +static int logicConditionGetAutoTransitionTargetStabilizedOperandValue(flight_dynamics_index_t axis) +{ + return getAutoTransitionTargetStabilizedInput(axis); +} +#endif + static int logicConditionGetFlightOperandValue(int operand) { switch (operand) { @@ -896,6 +903,20 @@ static int logicConditionGetFlightOperandValue(int operand) { return axisPID[PITCH]; break; +#ifdef USE_AUTO_TRANSITION + case LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_ROLL: + return logicConditionGetAutoTransitionTargetStabilizedOperandValue(FD_ROLL); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_PITCH: + return logicConditionGetAutoTransitionTargetStabilizedOperandValue(FD_PITCH); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_YAW: + return logicConditionGetAutoTransitionTargetStabilizedOperandValue(FD_YAW); + break; +#endif + case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT32_MAX); break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 34155ea082e..d4896025025 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -156,6 +156,11 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED, // cm/s // 47 LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION, // deg // 48 LOGIC_CONDITION_OPERAND_FLIGHT_RELATIVE_WIND_OFFSET, // deg // 49 +#ifdef USE_AUTO_TRANSITION + LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_ROLL, // 50 + LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_PITCH, // 51 + LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_YAW, // 52 +#endif } logicFlightOperands_e; typedef enum { diff --git a/src/main/target/common.h b/src/main/target/common.h index 55ca1653f5a..6b61357069a 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -201,8 +201,9 @@ #define USE_TELEMETRY_SBUS2 #endif -//Designed to free space of F722 and F411 MCUs +// Keep larger optional features off 512 KB targets to preserve flash space. #if (MCU_FLASH_SIZE > 512) +#define USE_AUTO_TRANSITION #define USE_TELEMETRY_SIM #define USE_VTX_FFPV #define USE_SERIALRX_SUMD @@ -228,4 +229,3 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER - From 8218e511a9d2a4154ba90f37db3f2ce2b7ca12d6 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Sun, 7 Jun 2026 15:46:39 +0300 Subject: [PATCH 26/42] Refine VTOL auto-transition preview and scaling docs - improve target fixed-wing servo preview during auto-transition - rename VTOL transition percentage settings to *_min_percent - separate motor ramp timing from transition handoff timing - update CLI-generated and narrative docs to match the behavior --- docs/MixerProfile.md | 41 +++--- docs/Settings.md | 16 +-- docs/VTOL.md | 110 +++++++-------- src/main/fc/config.c | 11 +- src/main/fc/config.h | 6 +- src/main/fc/settings.yaml | 22 +-- src/main/flight/mixer_profile.c | 31 ++-- src/main/flight/pid.c | 242 +++++++++++++++++++++++++++++++- 8 files changed, 362 insertions(+), 117 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 25b05fda5b2..6a6eb44cc2b 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -173,22 +173,22 @@ When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: Default is OFF to preserve existing behavior. With it ON, you can configure `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules in the MC mixer profile. -During MC->FW they drive the selected servo outputs from the target FW controller before the hot-switch. +During MC->FW they give those servos a preview of the fixed-wing stabilisation that will take over after the hot-switch. +This preview uses the target fixed-wing PID bank, rates, angle limits, heading-hold limits, and turn-assist gains, but it still follows the current transition stick shaping until the actual profile switch. During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. `INPUT_MIXER_TRANSITION` remains available for transition-progress servo movement such as tilt or helper servos. How `mixer_vtol_transition_scale_ramp_time_ms` works: -- MC->FW pusher: - - `> 0`: forward motor power ramps from `0 -> 100%` over this time, even when pitot is working normally. - - `= 0` (default): forward motor power goes to `100%` immediately. +- Motor ramp-in: + - MC->FW: forward motor power ramps from `0 -> 100%` over this time. + - FW->MC: lift motor power ramps from `vtol_transition_lift_min_percent -> 100%` over this time. + - `= 0` (default): those motor-power changes happen immediately. - This timer does not decide when the transition completes. -- Lift motor power, MC stabilisation, and FW control: - - with valid pitot airspeed, they still follow transition progress based on airspeed. - - if pitot stops being usable and this setting is `> 0`, they use this same timer as a backup ramp. - - if pitot stops being usable and this setting is `0`, they fall back to the normal transition timer/progress behavior. -- FW->MC keeps the existing style of smooth handover. +- Lift motor reduction in MC->FW, plus MC/FW control handoff in both directions: + - with valid pitot airspeed, they follow transition progress based on airspeed. + - without trusted pitot, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). Example: @@ -197,8 +197,9 @@ Example: Result: - in MC->FW, the forward motor reaches full power in about `1.2s`, -- when pitot is working, lift power and control handover still follow airspeed, -- if pitot stops being usable, the same handover reaches its target in about `1.2s`, +- in FW->MC, lift motor power returns to full in about `1.2s`, +- when pitot is working, control handover still follows airspeed, +- if pitot stops being usable, handover falls back to `mixer_switch_trans_timer`, - transition completion still uses airspeed when pitot is working, - backup completion time is still `5s` if pitot is not usable. @@ -224,9 +225,9 @@ Setting scope: - `vtol_transition_to_fw_min_airspeed_cm_s` - `vtol_transition_to_mc_max_airspeed_cm_s` - `vtol_fw_to_mc_auto_switch_airspeed_cm_s` - - `vtol_transition_lift_end_percent` - - `vtol_transition_mc_authority_end_percent` - - `vtol_transition_fw_authority_start_percent` + - `vtol_transition_lift_min_percent` + - `vtol_transition_mc_authority_min_percent` + - `vtol_transition_fw_authority_min_percent` - `nav_vtol_mission_transition_user_action` - `nav_vtol_mission_transition_min_altitude_cm` @@ -275,9 +276,9 @@ CLI: - `set mixer_switch_trans_timer = 50` - `set mixer_vtol_transition_airspeed_timeout_ms = 6500` - `set mixer_vtol_transition_scale_ramp_time_ms = 1200` -- `set vtol_transition_lift_end_percent = 30` -- `set vtol_transition_mc_authority_end_percent = 20` -- `set vtol_transition_fw_authority_start_percent = 20` +- `set vtol_transition_lift_min_percent = 30` +- `set vtol_transition_mc_authority_min_percent = 20` +- `set vtol_transition_fw_authority_min_percent = 20` - `set nav_vtol_mission_transition_user_action = OFF` Behavior: @@ -294,9 +295,9 @@ CLI: - `set mixer_switch_trans_timer = 50` - `set mixer_vtol_transition_airspeed_timeout_ms = 6500` - `set mixer_vtol_transition_scale_ramp_time_ms = 1200` -- `set vtol_transition_lift_end_percent = 30` -- `set vtol_transition_mc_authority_end_percent = 20` -- `set vtol_transition_fw_authority_start_percent = 20` +- `set vtol_transition_lift_min_percent = 30` +- `set vtol_transition_mc_authority_min_percent = 20` +- `set vtol_transition_fw_authority_min_percent = 20` - `set nav_vtol_mission_transition_user_action = USER1` - `set nav_vtol_mission_transition_min_altitude_cm = 1200` diff --git a/docs/Settings.md b/docs/Settings.md index 90ab68a7527..53a83477010 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3202,7 +3202,7 @@ If enabled, control_profile_index will follow mixer_profile index. Set to OFF(de ### mixer_switch_trans_timer -Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing. +Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing whenever trusted pitot is not usable. | Default | Min | Max | | --- | --- | --- | @@ -3242,7 +3242,7 @@ Turns on smooth VTOL transition power changes. This affects forward motor ramp-u ### mixer_vtol_transition_scale_ramp_time_ms -When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable. Available only on targets with more than 512 KB flash. +When smooth VTOL transition power changes are ON, this controls motor ramp-in time only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the lift motors from their configured minimum back to full power. `0` applies those motor-power changes immediately. This timer does not decide when the transition completes and it does not control the multicopter/fixed-wing control handoff. Handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7086,9 +7086,9 @@ Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to th --- -### vtol_transition_fw_authority_start_percent +### vtol_transition_fw_authority_min_percent -How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring it in and out more gently. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength. In FW->MC, it fades down from full strength to this value. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7096,9 +7096,9 @@ How much fixed-wing control is available at the start of transition, in percent. --- -### vtol_transition_lift_end_percent +### vtol_transition_lift_min_percent -How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest lift motor power used during transition, in percent. In MC->FW, lift power fades down to this value. In FW->MC, lift power starts from this value and rises back to full power. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7106,9 +7106,9 @@ How much lift motor power remains at the end of transition, in percent. `100` ke --- -### vtol_transition_mc_authority_end_percent +### vtol_transition_mc_authority_min_percent -How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest multicopter stabilisation used during transition, in percent. In MC->FW, multicopter stabilisation fades down to this value. In FW->MC, it starts from this value and rises back to full stabilisation. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md index 9895f1a03d5..df0aab8b91f 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -393,23 +393,23 @@ When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: When `mixer_vtol_transition_dynamic_mixer = OFF`, the older static behavior is preserved. When it is ON, you can configure `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules in the MC mixer profile. -During MC->FW they drive the selected servo outputs from the target FW controller before the hot-switch. +During MC->FW they give those servos a preview of the fixed-wing stabilisation that will take over after the hot-switch. +This preview uses the target fixed-wing PID bank, rates, angle limits, heading-hold limits, and turn-assist gains, but it still follows the current transition stick shaping until the actual profile switch. During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. `INPUT_MIXER_TRANSITION` remains available for transition-progress servo movement such as tilt or helper servos. -`mixer_vtol_transition_scale_ramp_time_ms` always controls the MC->FW forward-motor ramp when this feature is ON. +`mixer_vtol_transition_scale_ramp_time_ms` controls motor ramp-in timing when this feature is ON. It does not decide when the transition completes. How `mixer_vtol_transition_scale_ramp_time_ms` works: -- MC->FW pusher: - - `> 0`: forward motor power ramps from `0 -> 100%` over this time, even when pitot is working normally. - - `= 0` (default): forward motor power goes to `100%` immediately. -- Lift motor power, MC stabilisation, and FW control: +- Motor ramp-in: + - MC->FW: forward motor power ramps from `0 -> 100%` over this time. + - FW->MC: lift motor power ramps from `vtol_transition_lift_min_percent -> 100%` over this time. + - `= 0` (default): those motor-power changes happen immediately. +- Lift motor reduction in MC->FW, plus MC/FW control handoff in both directions: - with valid pitot airspeed, they still follow airspeed-based transition progress. - - if pitot stops being usable and this setting is `> 0`, they use this same timer as a backup ramp. - - if pitot stops being usable and this setting is `0`, they fall back to the normal transition timer/progress behavior. -- FW->MC keeps the existing style of smooth handover. + - if pitot is not usable, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). Example: - `mixer_switch_trans_timer = 50` (5s fallback completion timer) @@ -417,8 +417,9 @@ Example: Result: - in MC->FW, the forward motor reaches full power in about `1.2s`, -- when pitot is working, lift power and control handover still follow airspeed, -- if pitot stops being usable, the same handover reaches its target in about `1.2s`, +- in FW->MC, lift motor power returns to full in about `1.2s`, +- when pitot is working, control handover still follows airspeed, +- if pitot is not usable, handover falls back to `mixer_switch_trans_timer`, - transition completion still uses airspeed when pitot is working, - backup completion time is still `5s` if pitot is not usable. @@ -461,9 +462,9 @@ CLI: - `set mixer_switch_trans_timer = 50` - `set mixer_vtol_transition_airspeed_timeout_ms = 6500` - `set mixer_vtol_transition_scale_ramp_time_ms = 1200` -- `set vtol_transition_lift_end_percent = 30` -- `set vtol_transition_mc_authority_end_percent = 20` -- `set vtol_transition_fw_authority_start_percent = 20` +- `set vtol_transition_lift_min_percent = 30` +- `set vtol_transition_mc_authority_min_percent = 20` +- `set vtol_transition_fw_authority_min_percent = 20` - `set nav_vtol_mission_transition_user_action = OFF` What this does: @@ -486,9 +487,9 @@ CLI: - `set mixer_switch_trans_timer = 50` - `set mixer_vtol_transition_airspeed_timeout_ms = 6500` - `set mixer_vtol_transition_scale_ramp_time_ms = 1200` -- `set vtol_transition_lift_end_percent = 30` -- `set vtol_transition_mc_authority_end_percent = 20` -- `set vtol_transition_fw_authority_start_percent = 20` +- `set vtol_transition_lift_min_percent = 30` +- `set vtol_transition_mc_authority_min_percent = 20` +- `set vtol_transition_fw_authority_min_percent = 20` - `set nav_vtol_mission_transition_user_action = USER1` - `set nav_vtol_mission_transition_min_altitude_cm = 1200` @@ -504,37 +505,37 @@ What this does: These three settings are active only when `mixer_vtol_transition_dynamic_mixer = ON`. -1. `vtol_transition_lift_end_percent` -- Sets how much lift motor power remains at the end of transition. -- MC -> FW: lift power goes from `100%` at start to `lift_end_percent` at the end. -- FW -> MC: lift power goes from `lift_end_percent` at start to `100%` at the end. +1. `vtol_transition_lift_min_percent` +- Sets the lowest lift motor power used during transition. +- MC -> FW: lift power goes from `100%` at start down to `lift_min_percent`. +- FW -> MC: lift power goes from `lift_min_percent` at start up to `100%`. -Example (`vtol_transition_lift_end_percent = 20`): +Example (`vtol_transition_lift_min_percent = 20`): - MC -> FW at 50% progress: lift power is about `60%`. - FW -> MC at 50% progress: lift power is about `60%`. -2. `vtol_transition_mc_authority_end_percent` -- Sets how much multicopter stabilisation remains at the end of transition. -- MC -> FW: MC stabilisation goes from `100%` at start to `mc_authority_end_percent` at the end. -- FW -> MC: MC stabilisation goes from `mc_authority_end_percent` at start to `100%` at the end. +2. `vtol_transition_mc_authority_min_percent` +- Sets the lowest multicopter stabilisation used during transition. +- MC -> FW: MC stabilisation goes from `100%` at start down to `mc_authority_min_percent`. +- FW -> MC: MC stabilisation goes from `mc_authority_min_percent` at start up to `100%`. -Example (`vtol_transition_mc_authority_end_percent = 30`): +Example (`vtol_transition_mc_authority_min_percent = 30`): - MC -> FW at 50% progress: MC stabilisation is about `65%`. - FW -> MC at 50% progress: MC stabilisation is about `65%`. -3. `vtol_transition_fw_authority_start_percent` -- Sets how much fixed-wing control is already available at the start of transition. -- MC -> FW: fixed-wing control goes from `fw_authority_start_percent` at start to `100%` at the end. -- FW -> MC: fixed-wing control goes from `100%` at start to `fw_authority_start_percent` at the end. +3. `vtol_transition_fw_authority_min_percent` +- Sets the lowest fixed-wing control used during transition. +- MC -> FW: fixed-wing control goes from `fw_authority_min_percent` at start up to `100%`. +- FW -> MC: fixed-wing control goes from `100%` at start down to `fw_authority_min_percent`. - During MC -> FW, this same setting also scales `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules configured in the MC mixer profile. - During FW -> MC, the same setting scales down the matching FW servo stabilisation on the outputs marked by those MC mixer rules. -Example (`vtol_transition_fw_authority_start_percent = 25`): +Example (`vtol_transition_fw_authority_min_percent = 25`): - MC -> FW at 50% progress: fixed-wing control is about `62.5%`. - FW -> MC at 50% progress: fixed-wing control is about `62.5%`. -Backward-compatible note: -- `vtol_transition_fw_authority_start_percent = 100` keeps the older fixed-wing control behavior. +Practical note: +- `vtol_transition_fw_authority_min_percent = 100` keeps full fixed-wing control through the whole transition. - Lower values bring fixed-wing control in and out more gently. ## Setting Scope (Important) @@ -560,9 +561,9 @@ These are shared system-wide and are not profile-specific: - `vtol_transition_to_fw_min_airspeed_cm_s` - `vtol_transition_to_mc_max_airspeed_cm_s` - `vtol_fw_to_mc_auto_switch_airspeed_cm_s` -- `vtol_transition_lift_end_percent` -- `vtol_transition_mc_authority_end_percent` -- `vtol_transition_fw_authority_start_percent` +- `vtol_transition_lift_min_percent` +- `vtol_transition_mc_authority_min_percent` +- `vtol_transition_fw_authority_min_percent` - `nav_vtol_mission_transition_user_action` - `nav_vtol_mission_transition_min_altitude_cm` @@ -592,16 +593,16 @@ Use these commands in CLI (`set ...`, then `save`): - How long iNAV waits for required pitot airspeed before aborting. - `set mixer_vtol_transition_scale_ramp_time_ms = ` - - Ramp-up time for the forward motor, and backup ramp time for the other smooth transition power changes. + - Ramp-in time for the MC->FW forward motor and the FW->MC lift motors. -- `set vtol_transition_lift_end_percent = <0..100>` - - How much lift motor power remains at the end of transition. +- `set vtol_transition_lift_min_percent = <0..100>` + - Lowest lift motor power used during transition. -- `set vtol_transition_mc_authority_end_percent = <0..100>` - - How much multicopter stabilisation remains at the end of transition. +- `set vtol_transition_mc_authority_min_percent = <0..100>` + - Lowest multicopter stabilisation used during transition. -- `set vtol_transition_fw_authority_start_percent = <0..100>` - - How much fixed-wing control is already available at the start of transition. +- `set vtol_transition_fw_authority_min_percent = <0..100>` + - Lowest fixed-wing control used during transition. - `set nav_vtol_mission_transition_user_action = OFF|USER1|USER2|USER3|USER4` - Selects which waypoint USER flag tells iNAV to use MC or FW at each waypoint. @@ -644,23 +645,22 @@ Smooth transition power changes (`mixer_vtol_transition_dynamic_mixer = ON`) use - MC -> FW: - forward motor power ramps `0 -> 1` - - lift motor power ramps `1 -> vtol_transition_lift_end_percent` - - MC stabilisation ramps `1 -> vtol_transition_mc_authority_end_percent` - - FW control ramps `vtol_transition_fw_authority_start_percent -> 1` + - lift motor power ramps `1 -> vtol_transition_lift_min_percent` + - MC stabilisation ramps `1 -> vtol_transition_mc_authority_min_percent` + - FW control ramps `vtol_transition_fw_authority_min_percent -> 1` - FW -> MC: - forward motor power ramps `1 -> 0` - - lift motor power ramps `vtol_transition_lift_end_percent -> 1` - - MC stabilisation ramps `vtol_transition_mc_authority_end_percent -> 1` - - FW control ramps `1 -> vtol_transition_fw_authority_start_percent` + - lift motor power ramps `vtol_transition_lift_min_percent -> 1` + - MC stabilisation ramps `vtol_transition_mc_authority_min_percent -> 1` + - FW control ramps `1 -> vtol_transition_fw_authority_min_percent` -MC->FW uses separate forward-motor ramp-up and control handover behavior. +Motor ramp-in and control handover are separate. For MC->FW, forward motor power uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, the motor goes to full power immediately. +For FW->MC, lift motor power uses the same timer to rise from `vtol_transition_lift_min_percent` back to full power; if this is `0`, that lift power returns immediately. This timer does not decide when the transition completes. -Lift motor power, MC stabilisation, and FW control still prefer pitot-based transition progress whenever pitot is working. -If pitot stops being usable and `mixer_vtol_transition_scale_ramp_time_ms > 0`, those other changes fall back to the same timer-based ramp. -If pitot is not usable and `mixer_vtol_transition_scale_ramp_time_ms = 0`, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). -FW->MC keeps the existing style of smooth handover. +Lift motor reduction in MC->FW, plus MC stabilisation and FW control handoff in both directions, still prefer pitot-based transition progress whenever pitot is working. +If pitot is not usable, those handoff changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). For transition/pusher motors (`-2.0 < throttle < -1.0`), output is interpolated from idle to target: diff --git a/src/main/fc/config.c b/src/main/fc/config.c index f9254e41aa1..8cdca161e34 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -103,7 +103,12 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES ); +// Keep PG version split because USE_AUTO_TRANSITION changes the stored layout only on >512 KB targets. +#ifdef USE_AUTO_TRANSITION PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 9); +#else +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 7); +#endif PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, @@ -121,9 +126,9 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .vtolTransitionToFwMinAirspeed = SETTING_VTOL_TRANSITION_TO_FW_MIN_AIRSPEED_CM_S_DEFAULT, .vtolTransitionToMcMaxAirspeed = SETTING_VTOL_TRANSITION_TO_MC_MAX_AIRSPEED_CM_S_DEFAULT, .vtolFwToMcAutoSwitchAirspeed = SETTING_VTOL_FW_TO_MC_AUTO_SWITCH_AIRSPEED_CM_S_DEFAULT, - .vtolTransitionLiftEndPercent = SETTING_VTOL_TRANSITION_LIFT_END_PERCENT_DEFAULT, - .vtolTransitionMcAuthorityEndPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_END_PERCENT_DEFAULT, - .vtolTransitionFwAuthorityStartPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_START_PERCENT_DEFAULT, + .vtolTransitionLiftMinPercent = SETTING_VTOL_TRANSITION_LIFT_MIN_PERCENT_DEFAULT, + .vtolTransitionMcAuthorityMinPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_MIN_PERCENT_DEFAULT, + .vtolTransitionFwAuthorityMinPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_MIN_PERCENT_DEFAULT, #endif .craftName = SETTING_NAME_DEFAULT, .pilotName = SETTING_NAME_DEFAULT diff --git a/src/main/fc/config.h b/src/main/fc/config.h index fd433e9e657..b22e292b032 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -82,9 +82,9 @@ typedef struct systemConfig_s { uint16_t vtolTransitionToFwMinAirspeed; uint16_t vtolTransitionToMcMaxAirspeed; uint16_t vtolFwToMcAutoSwitchAirspeed; - uint8_t vtolTransitionLiftEndPercent; - uint8_t vtolTransitionMcAuthorityEndPercent; - uint8_t vtolTransitionFwAuthorityStartPercent; + uint8_t vtolTransitionLiftMinPercent; + uint8_t vtolTransitionMcAuthorityMinPercent; + uint8_t vtolTransitionFwAuthorityMinPercent; #endif char craftName[MAX_NAME_LENGTH + 1]; char pilotName[MAX_NAME_LENGTH + 1]; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a65788dd1f0..16cc5bcfb17 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1281,7 +1281,7 @@ groups: field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing." + description: "Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing whenever trusted pitot is not usable." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 @@ -1306,7 +1306,7 @@ groups: min: 0 max: 60000 - name: mixer_vtol_transition_scale_ramp_time_ms - description: "When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable. Available only on targets with more than 512 KB flash." + description: "When smooth VTOL transition power changes are ON, this controls motor ramp-in time only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the lift motors from their configured minimum back to full power. `0` applies those motor-power changes immediately. This timer does not decide when the transition completes and it does not control the multicopter/fixed-wing control handoff. Handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs @@ -4044,25 +4044,25 @@ groups: field: vtolFwToMcAutoSwitchAirspeed min: 0 max: 20000 - - name: vtol_transition_lift_end_percent - description: "How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + - name: vtol_transition_lift_min_percent + description: "Lowest lift motor power used during transition, in percent. In MC->FW, lift power fades down to this value. In FW->MC, lift power starts from this value and rises back to full power. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 - field: vtolTransitionLiftEndPercent + field: vtolTransitionLiftMinPercent min: 0 max: 100 - - name: vtol_transition_mc_authority_end_percent - description: "How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + - name: vtol_transition_mc_authority_min_percent + description: "Lowest multicopter stabilisation used during transition, in percent. In MC->FW, multicopter stabilisation fades down to this value. In FW->MC, it starts from this value and rises back to full stabilisation. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 - field: vtolTransitionMcAuthorityEndPercent + field: vtolTransitionMcAuthorityMinPercent min: 0 max: 100 - - name: vtol_transition_fw_authority_start_percent - description: "How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring it in and out more gently. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + - name: vtol_transition_fw_authority_min_percent + description: "Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength. In FW->MC, it fades down from full strength to this value. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 - field: vtolTransitionFwAuthorityStartPercent + field: vtolTransitionFwAuthorityMinPercent min: 0 max: 100 - name: name diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 67f2e025b56..8475f1ba29e 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -47,7 +47,12 @@ static bool manualTransitionSessionLatched; static bool manualFwToMcProtectionLatched; #endif +// Keep PG version split because USE_AUTO_TRANSITION changes the stored mixer profile layout only on >512 KB targets. +#ifdef USE_AUTO_TRANSITION PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 4); +#else +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); +#endif void pgResetFn_mixerProfiles(mixerProfile_t *instance) { @@ -197,7 +202,7 @@ static float blendScale(float from, float to, float progress) return from + (to - from) * constrainf(progress, 0.0f, 1.0f); } -static float getPusherRampProgress(void) +static float getMotorRampProgress(void) { if (!currentMixerConfig.vtolTransitionDynamicMixer) { return 1.0f; @@ -222,16 +227,8 @@ static float getHandoffScalingProgress(void) return mixerProfileAT.handoffScalingProgress; } - if (currentMixerConfig.vtolTransitionScaleRampTimeMs > 0) { - const float rampProgress = getPusherRampProgress(); - - // Preserve already-applied handoff scaling if pitot drops out mid-transition. - mixerProfileAT.handoffScalingProgress = MAX(mixerProfileAT.handoffScalingProgress, rampProgress); - return mixerProfileAT.handoffScalingProgress; - } - - // Last-resort compatibility path: with no trusted pitot and no dedicated - // scaling ramp configured, reuse transition progress/timer behavior. + // Preserve already-applied handoff scaling if pitot drops out mid-transition. + // Without trusted pitot, handoff returns to the normal transition timer/progress behavior. mixerProfileAT.handoffScalingProgress = MAX(mixerProfileAT.handoffScalingProgress, constrainf(mixerProfileAT.progress, 0.0f, 1.0f)); return mixerProfileAT.handoffScalingProgress; } @@ -317,21 +314,23 @@ static void updateTransitionScales(void) return; } - const float liftFloor = constrainf(systemConfig()->vtolTransitionLiftEndPercent / 100.0f, 0.0f, 1.0f); - const float mcFloor = constrainf(systemConfig()->vtolTransitionMcAuthorityEndPercent / 100.0f, 0.0f, 1.0f); - const float fwFloor = constrainf(systemConfig()->vtolTransitionFwAuthorityStartPercent / 100.0f, 0.0f, 1.0f); + const float liftFloor = constrainf(systemConfig()->vtolTransitionLiftMinPercent / 100.0f, 0.0f, 1.0f); + const float mcFloor = constrainf(systemConfig()->vtolTransitionMcAuthorityMinPercent / 100.0f, 0.0f, 1.0f); + const float fwFloor = constrainf(systemConfig()->vtolTransitionFwAuthorityMinPercent / 100.0f, 0.0f, 1.0f); const float handoffProgress = getHandoffScalingProgress(); if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { - const float pusherProgress = getPusherRampProgress(); + const float pusherProgress = getMotorRampProgress(); mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, pusherProgress); mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, handoffProgress); mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, handoffProgress); mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, handoffProgress); } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + const float liftRampProgress = getMotorRampProgress(); + mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, handoffProgress); - mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, handoffProgress); + mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, liftRampProgress); mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, handoffProgress); mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, handoffProgress); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f36f6912653..a334069a871 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -129,6 +129,8 @@ typedef struct { float rateTarget; float errorGyroIf; float errorGyroIfLimit; + pt1Filter_t angleFilterState; + rateLimitFilter_t axisAccelFilter; pt1Filter_t ptermLpfState; filter_t dtermLpfState; float previousRateTarget; @@ -174,6 +176,7 @@ static EXTENDED_FASTRAM autoTransitionTargetPidState_t autoTransitionTargetPidSt static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; static EXTENDED_FASTRAM uint8_t itermRelax; #ifdef USE_AUTO_TRANSITION +static EXTENDED_FASTRAM pt1Filter_t autoTransitionTargetHeadingHoldRateFilter; static EXTENDED_FASTRAM pt1Filter_t autoTransitionTargetTpaFilter; static EXTENDED_FASTRAM filterApplyFnPtr autoTransitionTargetDTermLpfFilterApplyFn; static EXTENDED_FASTRAM uint8_t autoTransitionTargetYawLpfHz; @@ -220,6 +223,9 @@ static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; static void applyItermLimiting(pidState_t *pidState); +static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination); +float pidRcCommandToRate(int16_t stick, uint8_t rate); +int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis); PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11); @@ -465,6 +471,7 @@ static void resetAutoTransitionTargetPidState(void) { memset(autoTransitionTargetPidState, 0, sizeof(autoTransitionTargetPidState)); memset(autoTransitionTargetAxisPID, 0, sizeof(autoTransitionTargetAxisPID)); + memset(&autoTransitionTargetHeadingHoldRateFilter, 0, sizeof(autoTransitionTargetHeadingHoldRateFilter)); memset(&autoTransitionTargetTpaFilter, 0, sizeof(autoTransitionTargetTpaFilter)); autoTransitionTargetControlProfileIndex = -1; autoTransitionTargetDTermLpfFilterApplyFn = (filterApplyFnPtr)nullFilterApply; @@ -643,6 +650,239 @@ static void updateAutoTransitionTargetPIDCoefficients(const controlConfig_t *con } } +static float calcAutoTransitionTargetHorizonRateMagnitude(const pidProfile_t *targetPidProfile) +{ + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH)); + const float mostDeflectedStickPos = constrain(MAX(stickPosAil, stickPosEle), 0, 500) / 500.0f; + const float modeTransitionStickPos = constrain(targetPidProfile->bank_fw.pid[PID_LEVEL].D, 0, 100) / 100.0f; + + if (modeTransitionStickPos <= 0.0f) { + return 1.0f; + } + + if (mostDeflectedStickPos <= modeTransitionStickPos) { + return mostDeflectedStickPos / modeTransitionStickPos; + } + + return 1.0f; +} + +static uint8_t getAutoTransitionTargetHeadingHoldState(const pidProfile_t *targetPidProfile) +{ + const float headingHoldCosLimit = + cos_approx(DECIDEGREES_TO_RADIANS(targetPidProfile->max_angle_inclination[FD_ROLL])) * + cos_approx(DECIDEGREES_TO_RADIANS(targetPidProfile->max_angle_inclination[FD_PITCH])); + + if (calculateCosTiltAngle() < headingHoldCosLimit) { + return HEADING_HOLD_DISABLED; + } + + const int navHeadingState = navigationGetHeadingControlState(); + if (navHeadingState != NAV_HEADING_CONTROL_NONE) { + if (navHeadingState == NAV_HEADING_CONTROL_AUTO) { + return HEADING_HOLD_ENABLED; + } + } else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) { + return HEADING_HOLD_ENABLED; + } + + return HEADING_HOLD_UPDATE_HEADING; +} + +static float pidAutoTransitionTargetHeadingHold(const pidProfile_t *targetPidProfile, float dT) +{ + int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget; + + if (error > 180) { + error -= 360; + } else if (error < -180) { + error += 360; + } + + float headingHoldRate = error * targetPidProfile->bank_fw.pid[PID_HEADING].P / 30.0f; + headingHoldRate = constrainf(headingHoldRate, -targetPidProfile->heading_hold_rate_limit, targetPidProfile->heading_hold_rate_limit); + headingHoldRate = pt1FilterApply4(&autoTransitionTargetHeadingHoldRateFilter, headingHoldRate, HEADING_HOLD_ERROR_LPF_FREQ, dT); + + return headingHoldRate; +} + +static float computeAutoTransitionTargetPidLevelTarget(const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis) +{ + float angleTarget; + +#ifdef USE_PROGRAMMING_FRAMEWORK + angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), targetPidProfile->max_angle_inclination[axis]); +#else + angleTarget = pidRcCommandToAngle(rcCommand[axis], targetPidProfile->max_angle_inclination[axis]); +#endif + + if (axis == FD_PITCH) { +#ifdef USE_FW_AUTOLAND + if (FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { +#else + if (FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { +#endif + angleTarget += scaleRange( + MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), + 0, + currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, + 0, + navConfig()->fw.minThrottleDownPitchAngle + ); + } + + angleTarget -= DEGREES_TO_DECIDEGREES(targetPidProfile->fixedWingLevelTrim); + } + + return angleTarget; +} + +static void pidAutoTransitionTargetLevel( + const float angleTarget, + autoTransitionTargetPidState_t *pidState, + const controlConfig_t *controlProfile, + const pidProfile_t *targetPidProfile, + flight_dynamics_index_t axis, + float horizonRateMagnitude, + float dT) +{ + float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); + + if (FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) { + angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)); + if (!angleErrorDeg) { + pidState->errorGyroIf = 0.0f; + pidState->errorGyroIfLimit = 0.0f; + } + } + + float angleRateTarget = constrainf( + angleErrorDeg * (targetPidProfile->bank_fw.pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), + -controlProfile->stabilized.rates[axis] * 10.0f, + controlProfile->stabilized.rates[axis] * 10.0f + ); + + if (targetPidProfile->bank_fw.pid[PID_LEVEL].I) { + angleRateTarget = pt1FilterApply4(&pidState->angleFilterState, angleRateTarget, targetPidProfile->bank_fw.pid[PID_LEVEL].I, dT); + } + + if (FLIGHT_MODE(HORIZON_MODE)) { + pidState->rateTarget = (1.0f - horizonRateMagnitude) * angleRateTarget + horizonRateMagnitude * pidState->rateTarget; + } else { + pidState->rateTarget = angleRateTarget; + } +} + +static void pidApplyAutoTransitionTargetSetpointRateLimiting( + autoTransitionTargetPidState_t *pidState, + const pidProfile_t *targetPidProfile, + flight_dynamics_index_t axis, + float dT) +{ + const uint32_t axisAccelLimit = (axis == FD_YAW) ? targetPidProfile->axisAccelerationLimitYaw : targetPidProfile->axisAccelerationLimitRollPitch; + + if (axisAccelLimit > AXIS_ACCEL_MIN_LIMIT) { + pidState->rateTarget = rateLimitFilterApply4(&pidState->axisAccelFilter, pidState->rateTarget, (float)axisAccelLimit, dT); + } +} + +static void pidAutoTransitionTargetTurnAssistant( + autoTransitionTargetPidState_t *pidState, + const controlConfig_t *controlProfile, + const pidProfile_t *targetPidProfile, + float bankAngleTarget, + float pitchAngleTarget) +{ + fpVector3_t targetRates; + targetRates.x = 0.0f; + targetRates.y = 0.0f; + + if (calculateCosTiltAngle() < 0.173648f) { + return; + } + +#if defined(USE_PITOT) + float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) && pitotIsHealthy() ? getAirspeedEstimate() : targetPidProfile->fixedWingReferenceAirspeed; +#else + float airspeedForCoordinatedTurn = targetPidProfile->fixedWingReferenceAirspeed; +#endif + + airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300.0f, 6000.0f); + bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60)); + + const float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget)); + const float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor; + + targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); + + imuTransformVectorEarthToBody(&targetRates); + + pidState[ROLL].rateTarget = constrainf( + pidState[ROLL].rateTarget + targetRates.x, + -controlProfile->stabilized.rates[ROLL] * 10.0f, + controlProfile->stabilized.rates[ROLL] * 10.0f + ); + pidState[PITCH].rateTarget = constrainf( + pidState[PITCH].rateTarget + targetRates.y * targetPidProfile->fixedWingCoordinatedPitchGain, + -controlProfile->stabilized.rates[PITCH] * 10.0f, + controlProfile->stabilized.rates[PITCH] * 10.0f + ); + pidState[YAW].rateTarget = constrainf( + pidState[YAW].rateTarget + targetRates.z * targetPidProfile->fixedWingCoordinatedYawGain, + -controlProfile->stabilized.rates[YAW] * 10.0f, + controlProfile->stabilized.rates[YAW] * 10.0f + ); +} + +static void updateAutoTransitionTargetRateTargets(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, float dT) +{ + const uint8_t headingHoldState = getAutoTransitionTargetHeadingHoldState(targetPidProfile); + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + float rateTarget; + + if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) { + rateTarget = pidAutoTransitionTargetHeadingHold(targetPidProfile, dT); + } else { +#ifdef USE_PROGRAMMING_FRAMEWORK + rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), controlProfile->stabilized.rates[axis]); +#else + rateTarget = pidRcCommandToRate(rcCommand[axis], controlProfile->stabilized.rates[axis]); +#endif + } + + autoTransitionTargetPidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + } + + const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcAutoTransitionTargetHorizonRateMagnitude(targetPidProfile) : 0.0f; + + for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) { + float angleTarget = getFlightAxisAngleOverride(axis, computeAutoTransitionTargetPidLevelTarget(targetPidProfile, axis)); + + if (STATE(TAILSITTER) && isMixerTransitionMixing && axis == FD_PITCH) { + angleTarget += DEGREES_TO_DECIDEGREES(45); + } + + // Preview the target fixed-wing angle controller using the current transition stick command. + // Airplane angle-hold target memory is not duplicated here, so this remains a close preview + // rather than a bit-for-bit copy of the post-switch controller state. + pidAutoTransitionTargetLevel(angleTarget, &autoTransitionTargetPidState[axis], controlProfile, targetPidProfile, axis, horizonRateMagnitude, dT); + } + } + + if (FLIGHT_MODE(TURN_ASSISTANT) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + const float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], targetPidProfile->max_angle_inclination[FD_ROLL])); + const float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], targetPidProfile->max_angle_inclination[FD_PITCH])); + pidAutoTransitionTargetTurnAssistant(autoTransitionTargetPidState, controlProfile, targetPidProfile, bankAngleTarget, pitchAngleTarget); + } + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + pidApplyAutoTransitionTargetSetpointRateLimiting(&autoTransitionTargetPidState[axis], targetPidProfile, axis, dT); + } +} + static float autoTransitionTargetPTermProcess(autoTransitionTargetPidState_t *pidState, flight_dynamics_index_t axis, float rateError, float dT) { const float newPTerm = rateError * pidState->kP; @@ -782,12 +1022,12 @@ static void updateAutoTransitionTargetAxisPID(float dT) } updateAutoTransitionTargetPIDCoefficients(controlProfile, targetPidProfile); + updateAutoTransitionTargetRateTargets(controlProfile, targetPidProfile, dT); const float dT_inv = 1.0f / dT; for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { autoTransitionTargetPidState[axis].gyroRate = gyro.gyroADCf[axis]; - autoTransitionTargetPidState[axis].rateTarget = pidState[axis].rateTarget; #ifdef USE_SMITH_PREDICTOR autoTransitionTargetPidState[axis].gyroRate = applySmithPredictor(axis, &autoTransitionTargetPidState[axis].smithPredictor, autoTransitionTargetPidState[axis].gyroRate); From 1dbaf49668a954fa7f18c59cd567f3075736410e Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 9 Jun 2026 13:08:03 +0300 Subject: [PATCH 27/42] Refine VTOL auto-transition mixer handoff and docs --- docs/MixerProfile.md | 8 +-- docs/Settings.md | 13 +++-- docs/VTOL.md | 12 ++--- src/main/fc/settings.yaml | 12 ++--- src/main/flight/mixer.c | 16 ++++-- src/main/flight/mixer_profile.c | 86 ++++++++++++++++++++++----------- src/main/flight/mixer_profile.h | 1 + src/main/flight/pid.c | 16 +++--- src/main/flight/servos.c | 12 +++-- 9 files changed, 112 insertions(+), 64 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 6a6eb44cc2b..5693b99237c 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -181,10 +181,10 @@ These inputs are active only while the smooth autotransition controller is runni How `mixer_vtol_transition_scale_ramp_time_ms` works: -- Motor ramp-in: +- Time-based motor/power handover: - MC->FW: forward motor power ramps from `0 -> 100%` over this time. - - FW->MC: lift motor power ramps from `vtol_transition_lift_min_percent -> 100%` over this time. - - `= 0` (default): those motor-power changes happen immediately. + - FW->MC: forward motor power ramps from `100% -> 0%`, while lift power and MC stabilisation ramp from their configured minimums back to `100%` over this time. + - `= 0` (default): those time-based power changes happen immediately. - This timer does not decide when the transition completes. - Lift motor reduction in MC->FW, plus MC/FW control handoff in both directions: - with valid pitot airspeed, they follow transition progress based on airspeed. @@ -197,7 +197,7 @@ Example: Result: - in MC->FW, the forward motor reaches full power in about `1.2s`, -- in FW->MC, lift motor power returns to full in about `1.2s`, +- in FW->MC, the forward motor ramps down while lift power and MC stabilisation return in about `1.2s`, - when pitot is working, control handover still follows airspeed, - if pitot stops being usable, handover falls back to `mixer_switch_trans_timer`, - transition completion still uses airspeed when pitot is working, diff --git a/docs/Settings.md b/docs/Settings.md index e597a2fe6f7..0b2fdd1cf59 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3212,7 +3212,7 @@ If enabled, control_profile_index will follow mixer_profile index. Set to OFF(de ### mixer_switch_trans_timer -Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing whenever trusted pitot is not usable. +Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, the control-handoff side of the transition also falls back to this timing whenever trusted pitot is not usable. | Default | Min | Max | | --- | --- | --- | @@ -3242,7 +3242,7 @@ Maximum wait time [ms] for the required pitot airspeed during an airspeed-contro ### mixer_vtol_transition_dynamic_mixer -Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash. +Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward motor up while lift power and multicopter stabilisation fade down and fixed-wing control fades in. In FW->MC it ramps the forward motor down while lift power and multicopter stabilisation ramp back in, and fixed-wing control fades down. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -3252,7 +3252,7 @@ Turns on smooth VTOL transition power changes. This affects forward motor ramp-u ### mixer_vtol_transition_scale_ramp_time_ms -When smooth VTOL transition power changes are ON, this controls motor ramp-in time only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the lift motors from their configured minimum back to full power. `0` applies those motor-power changes immediately. This timer does not decide when the transition completes and it does not control the multicopter/fixed-wing control handoff. Handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash. +When smooth VTOL transition power changes are ON, this controls the time-based motor/power handover only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the forward motor down to idle and ramps lift power plus multicopter stabilisation back in. `0` applies those time-based power changes immediately. This timer does not decide when the transition completes and it does not control fixed-wing control-surface handoff. Fixed-wing handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7110,7 +7110,7 @@ Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to th ### vtol_transition_fw_authority_min_percent -Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength. In FW->MC, it fades down from full strength to this value. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength as the handoff progresses. In FW->MC, it fades down from full strength to this value as the handoff progresses. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7120,7 +7120,7 @@ Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, f ### vtol_transition_lift_min_percent -Lowest lift motor power used during transition, in percent. In MC->FW, lift power fades down to this value. In FW->MC, lift power starts from this value and rises back to full power. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest lift motor power used during transition, in percent. In MC->FW, lift power fades down to this value as the handoff progresses. In FW->MC, lift power starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7130,7 +7130,7 @@ Lowest lift motor power used during transition, in percent. In MC->FW, lift powe ### vtol_transition_mc_authority_min_percent -Lowest multicopter stabilisation used during transition, in percent. In MC->FW, multicopter stabilisation fades down to this value. In FW->MC, it starts from this value and rises back to full stabilisation. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest multicopter stabilisation used during transition, in percent. In MC->FW, multicopter stabilisation fades down to this value as the handoff progresses. In FW->MC, it starts from this value and rises back to full stabilisation by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7311,4 +7311,3 @@ Defines rotation rate on YAW axis that UAV will try to archive on max. stick def | 20 | 1 | 180 | --- - diff --git a/docs/VTOL.md b/docs/VTOL.md index 2ded36f0208..590278f5255 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -399,14 +399,14 @@ During FW->MC the same MC mixer rules mark which FW servo outputs should fade do These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. `INPUT_MIXER_TRANSITION` remains available for transition-progress servo movement such as tilt or helper servos. -`mixer_vtol_transition_scale_ramp_time_ms` controls motor ramp-in timing when this feature is ON. +`mixer_vtol_transition_scale_ramp_time_ms` controls the time-based motor/power handover when this feature is ON. It does not decide when the transition completes. How `mixer_vtol_transition_scale_ramp_time_ms` works: -- Motor ramp-in: +- Time-based motor/power handover: - MC->FW: forward motor power ramps from `0 -> 100%` over this time. - - FW->MC: lift motor power ramps from `vtol_transition_lift_min_percent -> 100%` over this time. - - `= 0` (default): those motor-power changes happen immediately. + - FW->MC: forward motor power ramps from `100% -> 0%`, while lift power and MC stabilisation ramp from their configured minimums back to `100%` over this time. + - `= 0` (default): those time-based power changes happen immediately. - Lift motor reduction in MC->FW, plus MC/FW control handoff in both directions: - with valid pitot airspeed, they still follow airspeed-based transition progress. - if pitot is not usable, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). @@ -417,7 +417,7 @@ Example: Result: - in MC->FW, the forward motor reaches full power in about `1.2s`, -- in FW->MC, lift motor power returns to full in about `1.2s`, +- in FW->MC, the forward motor ramps down while lift power and MC stabilisation return in about `1.2s`, - when pitot is working, control handover still follows airspeed, - if pitot is not usable, handover falls back to `mixer_switch_trans_timer`, - transition completion still uses airspeed when pitot is working, @@ -657,7 +657,7 @@ Smooth transition power changes (`mixer_vtol_transition_dynamic_mixer = ON`) use Motor ramp-in and control handover are separate. For MC->FW, forward motor power uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, the motor goes to full power immediately. -For FW->MC, lift motor power uses the same timer to rise from `vtol_transition_lift_min_percent` back to full power; if this is `0`, that lift power returns immediately. +For FW->MC, the same timer ramps the forward motor down to idle while lift power and MC stabilisation rise back from their configured minimums; if this is `0`, those changes happen immediately. This timer does not decide when the transition completes. Lift motor reduction in MC->FW, plus MC stabilisation and FW control handoff in both directions, still prefer pitot-based transition progress whenever pitot is working. If pitot is not usable, those handoff changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e41b8247aa3..7c56b97997e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1278,13 +1278,13 @@ groups: field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing whenever trusted pitot is not usable." + description: "Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, the control-handoff side of the transition also falls back to this timing whenever trusted pitot is not usable." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 max: 200 - name: mixer_vtol_transition_dynamic_mixer - description: "Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash." + description: "Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward motor up while lift power and multicopter stabilisation fade down and fixed-wing control fades in. In FW->MC it ramps the forward motor down while lift power and multicopter stabilisation ramp back in, and fixed-wing control fades down. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: OFF field: mixer_config.vtolTransitionDynamicMixer @@ -1303,7 +1303,7 @@ groups: min: 0 max: 60000 - name: mixer_vtol_transition_scale_ramp_time_ms - description: "When smooth VTOL transition power changes are ON, this controls motor ramp-in time only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the lift motors from their configured minimum back to full power. `0` applies those motor-power changes immediately. This timer does not decide when the transition completes and it does not control the multicopter/fixed-wing control handoff. Handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash." + description: "When smooth VTOL transition power changes are ON, this controls the time-based motor/power handover only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the forward motor down to idle and ramps lift power plus multicopter stabilisation back in. `0` applies those time-based power changes immediately. This timer does not decide when the transition completes and it does not control fixed-wing control-surface handoff. Fixed-wing handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs @@ -4053,21 +4053,21 @@ groups: min: 0 max: 20000 - name: vtol_transition_lift_min_percent - description: "Lowest lift motor power used during transition, in percent. In MC->FW, lift power fades down to this value. In FW->MC, lift power starts from this value and rises back to full power. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + description: "Lowest lift motor power used during transition, in percent. In MC->FW, lift power fades down to this value as the handoff progresses. In FW->MC, lift power starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionLiftMinPercent min: 0 max: 100 - name: vtol_transition_mc_authority_min_percent - description: "Lowest multicopter stabilisation used during transition, in percent. In MC->FW, multicopter stabilisation fades down to this value. In FW->MC, it starts from this value and rises back to full stabilisation. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + description: "Lowest multicopter stabilisation used during transition, in percent. In MC->FW, multicopter stabilisation fades down to this value as the handoff progresses. In FW->MC, it starts from this value and rises back to full stabilisation by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionMcAuthorityMinPercent min: 0 max: 100 - name: vtol_transition_fw_authority_min_percent - description: "Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength. In FW->MC, it fades down from full strength to this value. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + description: "Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength as the handoff progresses. In FW->MC, it fades down from full strength to this value as the handoff progresses. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionFwAuthorityMinPercent diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 10a96272c42..90c22d2e4c0 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -635,9 +635,19 @@ void FAST_CODE mixTable(void) for (int i = 0; i < motorCount; i++) { float motorThrottle = mixerThrottleCommand * currentMixer[i].throttle; #ifdef USE_AUTO_TRANSITION - const float liftScale = isMixerTransitionMixing ? mixerATGetLiftScale() : 1.0f; - if (currentMixer[i].throttle > 0.0f) { - motorThrottle *= liftScale; + if (isMixerTransitionMixing && currentMixer[i].throttle > 0.0f) { + // During MC->FW, positive-throttle motors in the active multirotor + // profile are lift motors, so they fade with liftScale. + // During FW->MC, positive-throttle motors in the active airplane + // profile are forward propulsion, so they must decay with + // pusherScale instead of staying tied to airspeed progress. + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && + isMultirotorTypePlatform(currentMixerConfig.platformType)) { + motorThrottle *= mixerATGetLiftScale(); + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && + !isMultirotorTypePlatform(currentMixerConfig.platformType)) { + motorThrottle *= mixerATGetPusherScale(); + } } #endif diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 745f0a3c360..fa532329e87 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -115,16 +115,19 @@ void mixerConfigInit(void) servosInit(); mixerUpdateStateFlags(); mixerInit(); - if (currentMixerConfig.controlProfileLinking) - { - // LOG_INFO(PWM, "mixer switch pidInit"); + + if (currentMixerConfig.controlProfileLinking) { setConfigProfile(getConfigMixerProfile()); - pidInit(); - pidInitFilters(); - pidResetErrorAccumulators(); //should be safer to reset error accumulators - schedulePidGainsUpdate(); - navigationUsePIDs(); // set navigation pid gains } + + // Reinitialize the active controller on every mixer hot-switch so AUTO PID + // selection follows the new platform type and no stale FW/MC integrator + // state leaks across the completed transition. + pidInit(); + pidInitFilters(); + pidResetErrorAccumulators(); + schedulePidGainsUpdate(); + navigationUsePIDs(); } void setMixerProfileAT(void) @@ -140,6 +143,7 @@ void setMixerProfileAT(void) mixerProfileAT.transitionStartAirspeedCaptured = false; mixerProfileAT.progress = 0.0f; mixerProfileAT.handoffScalingProgress = 0.0f; + mixerProfileAT.motorRampProgress = 0.0f; mixerProfileAT.transitionStartAirspeedCmS = 0.0f; mixerProfileAT.blendToFw = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW ? 0.0f : 1.0f; mixerProfileAT.pusherScale = 1.0f; @@ -179,6 +183,7 @@ static void resetTransitionScales(void) { mixerProfileAT.progress = 0.0f; mixerProfileAT.handoffScalingProgress = 0.0f; + mixerProfileAT.motorRampProgress = 0.0f; mixerProfileAT.blendToFw = 0.0f; mixerProfileAT.pusherScale = 0.0f; mixerProfileAT.liftScale = 1.0f; @@ -190,6 +195,7 @@ static void setLegacyTransitionScales(void) { mixerProfileAT.progress = 1.0f; mixerProfileAT.handoffScalingProgress = 1.0f; + mixerProfileAT.motorRampProgress = 1.0f; mixerProfileAT.blendToFw = 1.0f; mixerProfileAT.pusherScale = 1.0f; mixerProfileAT.liftScale = 1.0f; @@ -205,15 +211,18 @@ static float blendScale(float from, float to, float progress) static float getMotorRampProgress(void) { if (!currentMixerConfig.vtolTransitionDynamicMixer) { + mixerProfileAT.motorRampProgress = 1.0f; return 1.0f; } if (currentMixerConfig.vtolTransitionScaleRampTimeMs <= 0) { + mixerProfileAT.motorRampProgress = 1.0f; return 1.0f; } const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; - return constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); + mixerProfileAT.motorRampProgress = constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); + return mixerProfileAT.motorRampProgress; } static float getHandoffScalingProgress(void) @@ -318,20 +327,20 @@ static void updateTransitionScales(void) const float mcFloor = constrainf(systemConfig()->vtolTransitionMcAuthorityMinPercent / 100.0f, 0.0f, 1.0f); const float fwFloor = constrainf(systemConfig()->vtolTransitionFwAuthorityMinPercent / 100.0f, 0.0f, 1.0f); const float handoffProgress = getHandoffScalingProgress(); + const float motorRampProgress = getMotorRampProgress(); if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { - const float pusherProgress = getMotorRampProgress(); - - mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, pusherProgress); + mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, motorRampProgress); mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, handoffProgress); mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, handoffProgress); mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, handoffProgress); } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { - const float liftRampProgress = getMotorRampProgress(); - - mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, handoffProgress); - mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, liftRampProgress); - mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, handoffProgress); + // In FW->MC, propulsion handover must not wait for airspeed-derived + // handoff progress. Ramp down the pusher and ramp up lift / MC + // stabilisation by time so the aircraft can actually slow down. + mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, motorRampProgress); + mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, motorRampProgress); + mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, motorRampProgress); mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, handoffProgress); } @@ -709,7 +718,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); - const uint32_t transitionDebugFlags = + uint32_t transitionDebugFlags = ((uint32_t)mixerProfileAT.direction & 0x3U) | (mixerATIsActive() ? 1U << 2 : 0U) | (isMixerTransitionMixing ? 1U << 3 : 0U) | @@ -729,18 +738,41 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) (IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) ? 1U << 19 : 0U) | (manualTransitionSessionLatched ? 1U << 20 : 0U); + const uint8_t targetInputMode = + (isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) ? + (FLIGHT_MODE(MANUAL_MODE) ? 1U : 2U) : + 0U; + const uint16_t progressScaled = lrintf(constrainf(mixerProfileAT.progress, 0.0f, 1.0f) * 1000.0f); + const uint16_t handoffScaled = lrintf(constrainf(mixerProfileAT.handoffScalingProgress, 0.0f, 1.0f) * 1000.0f); + const uint16_t motorRampScaled = lrintf(constrainf(mixerProfileAT.motorRampProgress, 0.0f, 1.0f) * 1000.0f); + const uint16_t pusherScaled = lrintf(constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f) * 1000.0f); + const uint16_t liftScaled = lrintf(constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) * 1000.0f); + const uint16_t mcAuthorityScaled = lrintf(constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f) * 1000.0f); + const uint16_t fwAuthorityScaled = lrintf(constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f) * 1000.0f); + + transitionDebugFlags |= (((uint32_t)currentMixerConfig.platformType & 0xFU) << 21); + transitionDebugFlags |= (((uint32_t)pidIndexGetType(PID_ROLL) & 0x3U) << 25); + transitionDebugFlags |= (((uint32_t)targetInputMode & 0x3U) << 27); + // VTOL transition debug channels (DEBUG_VTOL_TRANSITION): - // [0] phase, [1] request, [2] packed transition flags, [3] progress x1000, - // [4] pusherScale x1000, [5] liftScale x1000, [6] mcAuthorityScale x1000, - // [7] transition_PID_mmix_multiplier_pitch from currentMixerConfig + // [0] phase + // [1] request | (direction << 8) + // [2] packed transition flags | active platform type | active PID type | target input mode + // [3] raw transition progress x1000 + // [4] pusherScale x1000 + // [5] liftScale x1000 + // [6] mcAuthorityScale x1000 | (fwAuthorityScale x1000 << 16) + // [7] handoffProgress x1000 | (motorRampProgress x1000 << 16) DEBUG_SET(DEBUG_VTOL_TRANSITION, 0, mixerProfileAT.phase); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, mixerProfileAT.request); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, (int32_t)(((uint32_t)mixerProfileAT.request & 0xFFU) | (((uint32_t)mixerProfileAT.direction & 0xFFU) << 8))); DEBUG_SET(DEBUG_VTOL_TRANSITION, 2, (int32_t)transitionDebugFlags); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 3, lrintf(constrainf(mixerProfileAT.progress, 0.0f, 1.0f) * 1000.0f)); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 4, lrintf(constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f) * 1000.0f)); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 5, lrintf(constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) * 1000.0f)); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 6, lrintf(constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f) * 1000.0f)); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, currentMixerConfig.transition_PID_mmix_multiplier_pitch); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 3, progressScaled); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 4, pusherScaled); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 5, liftScaled); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 6, (int32_t)((uint32_t)mcAuthorityScaled | ((uint32_t)fwAuthorityScaled << 16))); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, (int32_t)((uint32_t)handoffScaled | ((uint32_t)motorRampScaled << 16))); if (!isMixerTransitionMixing) { resetTransitionScales(); diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 05a4a9d1bc8..5d1a4da3f93 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -79,6 +79,7 @@ typedef struct mixerProfileAT_s { bool transitionStartAirspeedCaptured; float progress; float handoffScalingProgress; + float motorRampProgress; float transitionStartAirspeedCmS; float blendToFw; float pusherScale; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6ed1cae44ef..c022c88e013 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1078,18 +1078,18 @@ int16_t getAutoTransitionTargetStabilizedInput(flight_dynamics_index_t axis) const float scale = currentMixerConfig.vtolTransitionDynamicMixer ? mixerATGetFwAuthorityScale() : 1.0f; - if (FLIGHT_MODE(MANUAL_MODE)) { - return lrintf(rcCommand[axis] * scale); - } - if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { - return lrintf(autoTransitionTargetAxisPID[axis] * scale); - } + if (FLIGHT_MODE(MANUAL_MODE)) { + return lrintf(rcCommand[axis] * scale); + } - if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { - return lrintf(axisPID[axis] * scale); + return lrintf(autoTransitionTargetAxisPID[axis] * scale); } + // FW->MC does not preview multirotor lift stabilisation through these + // inputs. Returning current FW axisPID here would incorrectly feed fixed- + // wing controller output into rules that users may expect to represent MC + // takeover. return 0; } #endif diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index a1e68b7ea0a..79e22260d97 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -386,6 +386,8 @@ void servoMixer(float dT) const bool autoTransitionInputsActive = isMixerTransitionMixing && mixerATIsActive() && mixerProfileAT.direction != MIXERAT_DIRECTION_NONE; + const bool autoTransitionTargetPreviewActive = autoTransitionInputsActive && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW; const bool scaleCurrentFwServoRules = autoTransitionInputsActive && mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && !isMultirotorTypePlatform(currentMixerConfig.platformType); @@ -417,9 +419,13 @@ void servoMixer(float dT) } #ifdef USE_AUTO_TRANSITION - input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL] = getAutoTransitionTargetStabilizedInput(FD_ROLL); - input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH] = getAutoTransitionTargetStabilizedInput(FD_PITCH); - input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW] = getAutoTransitionTargetStabilizedInput(FD_YAW); + // These preview inputs are only for pre-switch FW servo handoff. + // In FW->MC the same marked rules are used only as identifiers so the + // currently active FW servo rules can fade out; they do not become MC + // lift-motor stabilisation sources. + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL] = autoTransitionTargetPreviewActive ? getAutoTransitionTargetStabilizedInput(FD_ROLL) : 0; + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH] = autoTransitionTargetPreviewActive ? getAutoTransitionTargetStabilizedInput(FD_PITCH) : 0; + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW] = autoTransitionTargetPreviewActive ? getAutoTransitionTargetStabilizedInput(FD_YAW) : 0; #endif input[INPUT_STABILIZED_ROLL_PLUS] = constrain(input[INPUT_STABILIZED_ROLL], 0, 1000); From 6795b65b5aeb9ec4d94fe94c4fb68f9cfd9c683e Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 9 Jun 2026 14:27:03 +0300 Subject: [PATCH 28/42] Improve VTOL transition motor handoff and target PID preview Add direction-aware target PID preview for smooth VTOL transitions, including target MC motor stabilisation during FW->MC. Blend shared tilt motor throttle between current and target mixer rules to avoid double throttle, and keep post-switch propulsion fade/debug visibility documented. Clarify placeholder vs legacy helper motor setup, including `-1.000` placeholders for Configurator compatibility. --- docs/MixerProfile.md | 38 ++++- docs/Settings.md | 6 +- docs/VTOL.md | 46 ++++-- src/main/fc/settings.yaml | 6 +- src/main/flight/mixer.c | 131 ++++++++++++++-- src/main/flight/mixer_profile.c | 212 ++++++++++++++++++++++++-- src/main/flight/mixer_profile.h | 14 ++ src/main/flight/pid.c | 257 ++++++++++++++++++++++++-------- src/main/flight/pid.h | 1 + 9 files changed, 599 insertions(+), 112 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 5693b99237c..ced97b344fb 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -97,6 +97,8 @@ Example: This will spin motor number 5 (counting from 1) at 20%, in transition m mmix 4 -1.200 0.000 0.000 0.000 ``` +For smooth VTOL auto-transition, prefer configuring the forward motor as a normal positive-throttle motor in the FW mixer profile and reserving the same motor index in the MC profile with a placeholder. Use `throttle = -1.000` as the placeholder if Configurator removes zero-throttle motor rules. Values below about `-1.05` are treated as the older transition helper motor style, not as a plain placeholder. + ## RC mode settings It is recommend that the pilot uses a RC mode switch to activate modes or switch profiles. @@ -176,19 +178,36 @@ With it ON, you can configure `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo r During MC->FW they give those servos a preview of the fixed-wing stabilisation that will take over after the hot-switch. This preview uses the target fixed-wing PID bank, rates, angle limits, heading-hold limits, and turn-assist gains, but it still follows the current transition stick shaping until the actual profile switch. During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. +At the same time, target MC motor rules can fade in before the switch and use a target MC PID preview, so lift motors are not driven by the active FW/PIFF controller. These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. `INPUT_MIXER_TRANSITION` remains available for transition-progress servo movement such as tilt or helper servos. +Forward motor setup for smooth auto-transition: + +- Preferred setup: configure the forward motor as a normal positive-throttle motor in the FW mixer profile. +- Reserve the same motor index in the MC mixer profile with a placeholder if that motor is not used in MC flight. Use `throttle = -1.000`, `roll = 0`, `pitch = 0`, `yaw = 0` if Configurator removes zero-throttle motor rules. +- During MC->FW, iNAV can ramp that target FW motor rule in before the hot-switch. +- For FW->MC, configure the lift motors as normal positive-throttle motors in the MC mixer profile. If those motor indexes are not used in FW flight, reserve them in the FW mixer profile with placeholders. +- During FW->MC, iNAV can ramp those target MC lift motor rules in before the hot-switch and use target MC stabilisation for their roll/pitch/yaw correction. +- The older `-2.0 <= throttle <= -1.05` transition motor rule still works as a legacy/helper path, but it is not the preferred setup for smooth auto-transition. +- If a helper such as `throttle = -1.200` is used before the MC->FW switch, smooth auto-transition fades from that helper output to the real FW mixer output after the hot-switch. +- If the same physical tilt motor is configured with positive throttle in both profiles on the same index, iNAV blends the base throttle between the MC and FW mixer rules instead of adding both throttles together. The current-profile stabilisation fades out while the target-profile stabilisation fades in. + How `mixer_vtol_transition_scale_ramp_time_ms` works: - Time-based motor/power handover: - MC->FW: forward motor power ramps from `0 -> 100%` over this time. + - After the MC->FW profile switch, lift motors that are not used by the FW profile fade to idle over this same time instead of stopping immediately. - FW->MC: forward motor power ramps from `100% -> 0%`, while lift power and MC stabilisation ramp from their configured minimums back to `100%` over this time. + - After the FW->MC profile switch, a forward motor that is not used by the MC profile is kept fading to idle and cannot be reintroduced by FW throttle. - `= 0` (default): those time-based power changes happen immediately. - This timer does not decide when the transition completes. -- Lift motor reduction in MC->FW, plus MC/FW control handoff in both directions: +- Airspeed-based control handoff: + - in MC->FW, lift power reduction, MC stabilisation fade-out, and FW control fade-in follow airspeed when pitot is usable. + - in FW->MC, FW control fade-out follows airspeed when pitot is usable. - with valid pitot airspeed, they follow transition progress based on airspeed. - without trusted pitot, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). +- In FW->MC, forward motor removal, lift power ramp-in, and MC motor stabilisation ramp-in use the time-based motor ramp so they do not wait for airspeed to fall first. Example: @@ -326,7 +345,7 @@ For transition troubleshooting, use: Debug channels: -- `debug[0]` = transition phase (`0=IDLE`, `1=TRANSITION_INITIALIZE`, `2=TRANSITIONING`) +- `debug[0]` = transition phase (`0=IDLE`, `1=TRANSITION_INITIALIZE`, `2=TRANSITIONING`, `3=POST_SWITCH_FADE`) - `debug[1]` = active request (`MIXERAT_REQUEST_*` enum value) - `debug[2]` = packed transition flags: - bits 0-1: transition direction (`0=NONE`, `1=TO_FW`, `2=TO_MC`) @@ -346,11 +365,22 @@ Debug channels: - bit17: failsafe mode active - bit18: manual VTOL auto-transition controller effective after mission gating - bit19: RC `MIXERPROFILE` mode active + - bits 21-24: active platform type + - bits 25-26: active PID type (`pidIndexGetType(PID_ROLL)`) + - bits 27-28: target preview mode (`0=none`, `1=manual FW`, `2=target FW PID`, `3=target MC PID`) + - bit29: post-switch fade active - `debug[3]` = progress x1000 (`0..1000`) - `debug[4]` = pusher scale x1000 (`0..1000`) - `debug[5]` = lift scale x1000 (`0..1000`) -- `debug[6]` = MC stabilisation scale x1000 (`0..1000`) -- `debug[7]` = current mixer profile pitch transition PID multiplier (`transition_PID_mmix_multiplier_pitch`) +- `debug[6]` = packed scales: + - low 16 bits: MC stabilisation scale x1000 (`0..1000`) + - high 16 bits: FW control scale x1000 (`0..1000`) +- `debug[7]` = packed progress values: + - bits 0-9: handoff progress (`0..1000`) + - bits 10-19: motor ramp progress (`0..1000`) + - bits 20-29: post-switch fade progress (`0..1000`) + +Final motor outputs are available in the normal Blackbox motor traces. During post-switch fade, the old lift or pusher motor output should move smoothly toward idle there. ## 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/Settings.md b/docs/Settings.md index 0b2fdd1cf59..d7d33aeab9e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3242,7 +3242,7 @@ Maximum wait time [ms] for the required pitot airspeed during an airspeed-contro ### mixer_vtol_transition_dynamic_mixer -Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward motor up while lift power and multicopter stabilisation fade down and fixed-wing control fades in. In FW->MC it ramps the forward motor down while lift power and multicopter stabilisation ramp back in, and fixed-wing control fades down. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash. +Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward motor up while lift power and multicopter stabilisation fade down and fixed-wing control fades in. In FW->MC it ramps the forward motor down while target MC lift power and target MC motor stabilisation ramp back in, and fixed-wing control fades down. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -3252,7 +3252,7 @@ Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward ### mixer_vtol_transition_scale_ramp_time_ms -When smooth VTOL transition power changes are ON, this controls the time-based motor/power handover only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the forward motor down to idle and ramps lift power plus multicopter stabilisation back in. `0` applies those time-based power changes immediately. This timer does not decide when the transition completes and it does not control fixed-wing control-surface handoff. Fixed-wing handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash. +When smooth VTOL transition power changes are ON, this controls the time-based motor/power handover only. In MC->FW it ramps the forward motor from idle to full target power. After the switch to FW, any old lift motors that are no longer used by the FW profile fade to idle over the same time. In FW->MC it ramps the forward motor down to idle and ramps target MC lift power plus target MC motor stabilisation back in. After the switch to MC, any old forward motor that is no longer used by the MC profile is kept fading to idle. `0` applies those time-based power changes immediately. This timer does not decide when the transition completes and it does not control fixed-wing control-surface handoff. Fixed-wing handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7130,7 +7130,7 @@ Lowest lift motor power used during transition, in percent. In MC->FW, lift powe ### vtol_transition_mc_authority_min_percent -Lowest multicopter stabilisation used during transition, in percent. In MC->FW, multicopter stabilisation fades down to this value as the handoff progresses. In FW->MC, it starts from this value and rises back to full stabilisation by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest multicopter stabilisation used during transition, in percent. In MC->FW, active MC motor stabilisation fades down to this value as the handoff progresses. In FW->MC, target MC motor stabilisation starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md index 590278f5255..89b106969f1 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -259,7 +259,7 @@ Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weigh ## Motor 'Transition Mixing': Dedicated forward motor configuration In motor mixer set: -- -2.0 < throttle < -1.0: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated. +- -2.0 <= throttle <= -1.05: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated. Use exactly `-1.000` only as a placeholder, not as a transition helper motor. - Airmode type should be set to "STICK_CENTER". Airmode type must NOT be set to "THROTTLE_THRESHOLD". If set to throttle threshold the (-) motor will spin until the throttle threshold is passed. ![Alt text](Screenshots/mixerprofile_4puls1_mix.png) @@ -396,20 +396,36 @@ When it is ON, you can configure `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` serv During MC->FW they give those servos a preview of the fixed-wing stabilisation that will take over after the hot-switch. This preview uses the target fixed-wing PID bank, rates, angle limits, heading-hold limits, and turn-assist gains, but it still follows the current transition stick shaping until the actual profile switch. During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. +At the same time, target MC motor rules can fade in before the switch and use a target MC PID preview, so lift motors are not driven by the active FW/PIFF controller. These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. `INPUT_MIXER_TRANSITION` remains available for transition-progress servo movement such as tilt or helper servos. +Forward motor setup for smooth auto-transition: + +- Preferred setup: configure the forward motor as a normal positive-throttle motor in the FW mixer profile. +- Reserve the same motor index in the MC mixer profile with a placeholder if that motor is not used in MC flight. Use `throttle = -1.000`, `roll = 0`, `pitch = 0`, `yaw = 0` if Configurator removes zero-throttle motor rules. +- During MC->FW, iNAV can ramp that target FW motor rule in before the hot-switch. +- For FW->MC, configure the lift motors as normal positive-throttle motors in the MC mixer profile. If those motor indexes are not used in FW flight, reserve them in the FW mixer profile with placeholders. +- During FW->MC, iNAV can ramp those target MC lift motor rules in before the hot-switch and use target MC stabilisation for their roll/pitch/yaw correction. +- The older `-2.0 <= throttle <= -1.05` transition motor rule still works as a legacy/helper path, but it is not the preferred setup for smooth auto-transition. +- If a helper such as `throttle = -1.200` is used before the MC->FW switch, smooth auto-transition fades from that helper output to the real FW mixer output after the hot-switch. +- If the same physical tilt motor is configured with positive throttle in both profiles on the same index, iNAV blends the base throttle between the MC and FW mixer rules instead of adding both throttles together. The current-profile stabilisation fades out while the target-profile stabilisation fades in. + `mixer_vtol_transition_scale_ramp_time_ms` controls the time-based motor/power handover when this feature is ON. It does not decide when the transition completes. How `mixer_vtol_transition_scale_ramp_time_ms` works: - Time-based motor/power handover: - MC->FW: forward motor power ramps from `0 -> 100%` over this time. + - After the MC->FW profile switch, lift motors that are not used by the FW profile fade to idle over this same time instead of stopping immediately. - FW->MC: forward motor power ramps from `100% -> 0%`, while lift power and MC stabilisation ramp from their configured minimums back to `100%` over this time. + - After the FW->MC profile switch, a forward motor that is not used by the MC profile is kept fading to idle and cannot be reintroduced by FW throttle. - `= 0` (default): those time-based power changes happen immediately. -- Lift motor reduction in MC->FW, plus MC/FW control handoff in both directions: - - with valid pitot airspeed, they still follow airspeed-based transition progress. - - if pitot is not usable, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). +- Airspeed-based control handoff: + - in MC->FW, lift power reduction, MC stabilisation fade-out, and FW control fade-in follow airspeed when pitot is usable. + - in FW->MC, FW control fade-out follows airspeed when pitot is usable. + - if pitot is not usable, those handoff changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). +- In FW->MC, forward motor removal, lift power ramp-in, and MC motor stabilisation ramp-in use the time-based motor ramp so they do not wait for airspeed to fall first. Example: - `mixer_switch_trans_timer = 50` (5s fallback completion timer) @@ -516,8 +532,9 @@ Example (`vtol_transition_lift_min_percent = 20`): 2. `vtol_transition_mc_authority_min_percent` - Sets the lowest multicopter stabilisation used during transition. -- MC -> FW: MC stabilisation goes from `100%` at start down to `mc_authority_min_percent`. -- FW -> MC: MC stabilisation goes from `mc_authority_min_percent` at start up to `100%`. +- MC -> FW: active MC motor stabilisation goes from `100%` at start down to `mc_authority_min_percent`. +- FW -> MC: target MC motor stabilisation goes from `mc_authority_min_percent` at start up to `100%`. +- During FW -> MC, this target MC stabilisation comes from the MC mixer profile and target MC PID preview, not from the active FW/PIFF controller. Example (`vtol_transition_mc_authority_min_percent = 30`): - MC -> FW at 50% progress: MC stabilisation is about `65%`. @@ -593,7 +610,7 @@ Use these commands in CLI (`set ...`, then `save`): - How long iNAV waits for required pitot airspeed before aborting. - `set mixer_vtol_transition_scale_ramp_time_ms = ` - - Ramp-in time for the MC->FW forward motor and the FW->MC lift motors. + - Ramp time for MC->FW forward motor power, FW->MC forward motor removal, and the short post-switch fade of old lift/pusher outputs. - `set vtol_transition_lift_min_percent = <0..100>` - Lowest lift motor power used during transition. @@ -655,14 +672,21 @@ Smooth transition power changes (`mixer_vtol_transition_dynamic_mixer = ON`) use - MC stabilisation ramps `vtol_transition_mc_authority_min_percent -> 1` - FW control ramps `1 -> vtol_transition_fw_authority_min_percent` +After the profile switch, iNAV keeps only the old propulsion output alive for a short fade: + +- MC -> FW: lift motors that are not used by the FW profile fade to idle. +- FW -> MC: a forward motor that is not used by the MC profile keeps fading to idle. +- This post-switch fade uses `mixer_vtol_transition_scale_ramp_time_ms` and does not keep the old PID/controller active. + Motor ramp-in and control handover are separate. For MC->FW, forward motor power uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, the motor goes to full power immediately. For FW->MC, the same timer ramps the forward motor down to idle while lift power and MC stabilisation rise back from their configured minimums; if this is `0`, those changes happen immediately. This timer does not decide when the transition completes. -Lift motor reduction in MC->FW, plus MC stabilisation and FW control handoff in both directions, still prefer pitot-based transition progress whenever pitot is working. -If pitot is not usable, those handoff changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). +In MC->FW, lift power reduction, MC stabilisation fade-out, and FW control fade-in still prefer pitot-based transition progress whenever pitot is working. +In FW->MC, FW control fade-out still prefers pitot-based transition progress, while forward motor removal, lift power ramp-in, and MC motor stabilisation ramp-in use the time-based motor ramp. +If pitot is not usable, the airspeed-based handoff changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). -For transition/pusher motors (`-2.0 < throttle < -1.0`), output is interpolated from idle to target: +For legacy/helper transition motors (`-2.0 <= throttle <= -1.05`), output is interpolated from idle to target: `motor = idle + (target - idle) * pusherScale` @@ -671,3 +695,5 @@ where: - `idle = throttleRangeMin` If pitot is unavailable/unhealthy, timer fallback is used (`mixer_switch_trans_timer`). + +For smooth auto-transition, the preferred forward motor setup is a normal positive-throttle rule in the FW mixer profile, with a placeholder on the same motor index in the MC mixer profile. Use `throttle = -1.000` for that placeholder if Configurator removes zero-throttle motor rules. If a helper such as `throttle = -1.200` is used, iNAV fades from that helper output to the real FW mixer output after the hot-switch. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7c56b97997e..fbe2d5207d4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1284,7 +1284,7 @@ groups: min: 0 max: 200 - name: mixer_vtol_transition_dynamic_mixer - description: "Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward motor up while lift power and multicopter stabilisation fade down and fixed-wing control fades in. In FW->MC it ramps the forward motor down while lift power and multicopter stabilisation ramp back in, and fixed-wing control fades down. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash." + description: "Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward motor up while lift power and multicopter stabilisation fade down and fixed-wing control fades in. In FW->MC it ramps the forward motor down while target MC lift power and target MC motor stabilisation ramp back in, and fixed-wing control fades down. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: OFF field: mixer_config.vtolTransitionDynamicMixer @@ -1303,7 +1303,7 @@ groups: min: 0 max: 60000 - name: mixer_vtol_transition_scale_ramp_time_ms - description: "When smooth VTOL transition power changes are ON, this controls the time-based motor/power handover only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the forward motor down to idle and ramps lift power plus multicopter stabilisation back in. `0` applies those time-based power changes immediately. This timer does not decide when the transition completes and it does not control fixed-wing control-surface handoff. Fixed-wing handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash." + description: "When smooth VTOL transition power changes are ON, this controls the time-based motor/power handover only. In MC->FW it ramps the forward motor from idle to full target power. After the switch to FW, any old lift motors that are no longer used by the FW profile fade to idle over the same time. In FW->MC it ramps the forward motor down to idle and ramps target MC lift power plus target MC motor stabilisation back in. After the switch to MC, any old forward motor that is no longer used by the MC profile is kept fading to idle. `0` applies those time-based power changes immediately. This timer does not decide when the transition completes and it does not control fixed-wing control-surface handoff. Fixed-wing handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs @@ -4060,7 +4060,7 @@ groups: min: 0 max: 100 - name: vtol_transition_mc_authority_min_percent - description: "Lowest multicopter stabilisation used during transition, in percent. In MC->FW, multicopter stabilisation fades down to this value as the handoff progresses. In FW->MC, it starts from this value and rises back to full stabilisation by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + description: "Lowest multicopter stabilisation used during transition, in percent. In MC->FW, active MC motor stabilisation fades down to this value as the handoff progresses. In FW->MC, target MC motor stabilisation starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionMcAuthorityMinPercent diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 90c22d2e4c0..459b4e085fa 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -523,10 +523,12 @@ void FAST_CODE mixTable(void) input[YAW] = axisPID[YAW]; if (isMixerTransitionMixing) { #ifdef USE_AUTO_TRANSITION - const float mcAuthorityScale = mixerATGetMcAuthorityScale(); - input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f) * mcAuthorityScale; - input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f) * mcAuthorityScale; - input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f) * mcAuthorityScale; + const float activeAuthorityScale = isMultirotorTypePlatform(currentMixerConfig.platformType) ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f) * activeAuthorityScale; + input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f) * activeAuthorityScale; + input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f) * activeAuthorityScale; #else input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f); input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f); @@ -535,6 +537,41 @@ void FAST_CODE mixTable(void) } } +#ifdef USE_AUTO_TRANSITION + const bool autoTransitionMotorMixing = isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction != MIXERAT_DIRECTION_NONE && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT; + const bool currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); + const int targetMixerProfileIndex = autoTransitionMotorMixing ? nextMixerProfileIndex : -1; + const motorMixer_t *targetMotorMixer = autoTransitionMotorMixing ? mixerMotorMixersByIndex(targetMixerProfileIndex) : NULL; + const bool targetProfileIsMultirotor = autoTransitionMotorMixing ? + isMultirotorTypePlatform(mixerConfigByIndex(targetMixerProfileIndex)->platformType) : + false; + int16_t targetInput[3] = { 0, 0, 0 }; + + if (autoTransitionMotorMixing) { + const float targetAuthorityScale = targetProfileIsMultirotor ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + + if (!targetProfileIsMultirotor && FLIGHT_MODE(MANUAL_MODE)) { + targetInput[ROLL] = rcCommand[ROLL]; + targetInput[PITCH] = rcCommand[PITCH]; + targetInput[YAW] = rcCommand[YAW]; + } else { + targetInput[ROLL] = getAutoTransitionTargetAxisPID(FD_ROLL); + targetInput[PITCH] = getAutoTransitionTargetAxisPID(FD_PITCH); + targetInput[YAW] = getAutoTransitionTargetAxisPID(FD_YAW); + } + + targetInput[ROLL] = targetInput[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f) * targetAuthorityScale; + targetInput[PITCH] = targetInput[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f) * targetAuthorityScale; + targetInput[YAW] = targetInput[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f) * targetAuthorityScale; + } +#endif + // Initial mixer concept by bdoiron74 reused and optimized for Air Mode int16_t rpyMix[MAX_SUPPORTED_MOTORS]; int16_t rpyMixMax = 0; // assumption: symetrical about zero. @@ -542,10 +579,28 @@ void FAST_CODE mixTable(void) // motors for non-servo mixes for (int i = 0; i < motorCount; i++) { +#ifdef USE_AUTO_TRANSITION + const motorMixer_t *targetMixer = autoTransitionMotorMixing ? &targetMotorMixer[i] : NULL; + const bool currentMotorActive = currentMixer[i].throttle > 0.0f; + const bool targetMotorActive = targetMixer && targetMixer->throttle > 0.0f; + const int16_t activeRpyMix = currentMotorActive ? + (input[PITCH] * currentMixer[i].pitch + + input[ROLL] * currentMixer[i].roll + + -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) : + 0; + const int16_t targetRpyMix = targetMotorActive ? + (targetInput[PITCH] * targetMixer->pitch + + targetInput[ROLL] * targetMixer->roll + + -motorYawMultiplier * targetInput[YAW] * targetMixer->yaw) : + 0; + + rpyMix[i] = (activeRpyMix + targetRpyMix) * mixerScale; +#else rpyMix[i] = (input[PITCH] * currentMixer[i].pitch + input[ROLL] * currentMixer[i].roll + -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale; +#endif if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i]; if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i]; @@ -635,18 +690,32 @@ void FAST_CODE mixTable(void) for (int i = 0; i < motorCount; i++) { float motorThrottle = mixerThrottleCommand * currentMixer[i].throttle; #ifdef USE_AUTO_TRANSITION - if (isMixerTransitionMixing && currentMixer[i].throttle > 0.0f) { - // During MC->FW, positive-throttle motors in the active multirotor - // profile are lift motors, so they fade with liftScale. - // During FW->MC, positive-throttle motors in the active airplane - // profile are forward propulsion, so they must decay with - // pusherScale instead of staying tied to airspeed progress. - if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && - isMultirotorTypePlatform(currentMixerConfig.platformType)) { - motorThrottle *= mixerATGetLiftScale(); - } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && - !isMultirotorTypePlatform(currentMixerConfig.platformType)) { - motorThrottle *= mixerATGetPusherScale(); + const motorMixer_t *targetMixer = autoTransitionMotorMixing ? &targetMotorMixer[i] : NULL; + const bool currentMotorActive = currentMixer[i].throttle > 0.0f; + const bool targetMotorActive = targetMixer && targetMixer->throttle > 0.0f; + const bool sharedMotor = currentMotorActive && targetMotorActive; + const bool currentMotorPlaceholder = currentMixer[i].throttle <= 0.0f && currentMixer[i].throttle > -1.05f; + const float pusherScale = mixerATGetPusherScale(); + + if (autoTransitionMotorMixing && currentMotorActive) { + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && currentProfileIsMultirotor) { + if (sharedMotor && !targetProfileIsMultirotor) { + const float propulsionBlend = pusherScale; + const float blendedThrottle = currentMixer[i].throttle * (1.0f - propulsionBlend) + + targetMixer->throttle * propulsionBlend; + motorThrottle = mixerThrottleCommand * blendedThrottle; + } else { + motorThrottle *= mixerATGetLiftScale(); + } + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && !currentProfileIsMultirotor) { + if (sharedMotor && targetProfileIsMultirotor) { + const float mcBlend = 1.0f - pusherScale; + const float blendedThrottle = currentMixer[i].throttle * pusherScale + + targetMixer->throttle * mcBlend; + motorThrottle = mixerThrottleCommand * blendedThrottle; + } else { + motorThrottle *= pusherScale; + } } } #endif @@ -663,6 +732,30 @@ void FAST_CODE mixTable(void) if (currentMixer[i].throttle <= 0.0f) { motor[i] = motorZeroCommand; } +#ifdef USE_AUTO_TRANSITION + if (autoTransitionMotorMixing && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && + currentProfileIsMultirotor && + currentMotorPlaceholder && + targetMotorActive && + !targetProfileIsMultirotor) { + // Smooth auto-transition pusher preview comes from the target FW + // mixer rule. A zero current rule reserves the physical motor index. + const float targetMotorThrottle = mixerThrottleCommand * targetMixer->throttle * pusherScale; + motor[i] = constrain(rpyMix[i] + constrain(targetMotorThrottle, throttleMin, throttleMax), throttleRangeMin, throttleRangeMax); + } else if (autoTransitionMotorMixing && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && + !currentProfileIsMultirotor && + currentMotorPlaceholder && + targetMotorActive && + targetProfileIsMultirotor) { + // Target MC lift preview uses the target MC mixer rule and target + // MC PID preview. This avoids feeding active FW/PIFF PID into lift + // motors while still allowing MC control to fade in before switch. + const float targetMotorThrottle = mixerThrottleCommand * targetMixer->throttle * mixerATGetLiftScale(); + motor[i] = constrain(rpyMix[i] + constrain(targetMotorThrottle, throttleMin, throttleMax), throttleRangeMin, throttleRangeMax); + } +#endif //spin stopped motors only in mixer transition mode if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && !feature(FEATURE_REVERSIBLE_MOTORS)) { #ifdef USE_AUTO_TRANSITION @@ -675,6 +768,12 @@ void FAST_CODE mixTable(void) #endif motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } +#ifdef USE_AUTO_TRANSITION + int16_t postSwitchFadeOutput = 0; + if (mixerATGetPostSwitchFadeMotorOutput(i, motorZeroCommand, motor[i], &postSwitchFadeOutput)) { + motor[i] = constrain(postSwitchFadeOutput, motorZeroCommand, getMaxThrottle()); + } +#endif } } diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index fa532329e87..82eaf0e49f7 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -150,6 +150,13 @@ void setMixerProfileAT(void) mixerProfileAT.liftScale = 1.0f; mixerProfileAT.mcAuthorityScale = 1.0f; mixerProfileAT.fwAuthorityScale = 1.0f; + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); #else mixerProfileAT.transitionStartTime = millis(); mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; @@ -189,6 +196,13 @@ static void resetTransitionScales(void) mixerProfileAT.liftScale = 1.0f; mixerProfileAT.mcAuthorityScale = 1.0f; mixerProfileAT.fwAuthorityScale = 1.0f; + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); } static void setLegacyTransitionScales(void) @@ -201,6 +215,10 @@ static void setLegacyTransitionScales(void) mixerProfileAT.liftScale = 1.0f; mixerProfileAT.mcAuthorityScale = 1.0f; mixerProfileAT.fwAuthorityScale = 1.0f; + mixerProfileAT.postSwitchFadeProgress = 1.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; } static float blendScale(float from, float to, float progress) @@ -280,6 +298,121 @@ static bool hasPitotSensorForManualProtection(void) #endif } +static void preparePostSwitchFade(const int targetProfileIndex) +{ + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); + + if (!currentMixerConfig.vtolTransitionDynamicMixer || + currentMixerConfig.vtolTransitionScaleRampTimeMs == 0 || + mixerProfileAT.direction == MIXERAT_DIRECTION_NONE || + targetProfileIndex < 0 || + targetProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + return; + } + + const motorMixer_t *currentMotorMixer = mixerMotorMixersByIndex(currentMixerProfileIndex); + const motorMixer_t *targetMotorMixer = mixerMotorMixersByIndex(targetProfileIndex); + const bool currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); + const uint8_t count = getMotorCount(); + + for (uint8_t i = 0; i < count && i < MAX_SUPPORTED_MOTORS; i++) { + const bool currentMotorActive = currentMotorMixer[i].throttle > 0.0f; + const bool targetMotorActive = targetMotorMixer[i].throttle > 0.0f; + // Fade only propulsion outputs that disappear after the hot-switch. + // Shared tilt motors are handled by complementary throttle blending and + // must not receive an extra captured-output overlay. + const bool oldLiftMotor = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && + currentProfileIsMultirotor && + currentMotorActive && + !targetMotorActive; + const bool oldPusherMotor = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && + !currentProfileIsMultirotor && + currentMotorActive && + !targetMotorActive; + const bool targetFwPusherMotor = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && + currentProfileIsMultirotor && + !currentMotorActive && + targetMotorActive; + + if (oldLiftMotor || oldPusherMotor || targetFwPusherMotor) { + mixerProfileAT.postSwitchFadeMotorOutput[i] = motor[i]; + mixerProfileAT.postSwitchFadeMotorMask |= (1U << i); + if (targetFwPusherMotor) { + mixerProfileAT.postSwitchFadeToCurrentMotorMask |= (1U << i); + } + } + } + + if (mixerProfileAT.postSwitchFadeMotorMask == 0) { + return; + } + + mixerProfileAT.postSwitchFadeDurationMs = currentMixerConfig.vtolTransitionScaleRampTimeMs; + mixerProfileAT.postSwitchFadeInitialScale = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW ? + constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) : + constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f); +} + +static bool startPostSwitchFade(void) +{ + if (mixerProfileAT.postSwitchFadeMotorMask == 0 || + mixerProfileAT.postSwitchFadeDurationMs == 0) { + return false; + } + + mixerProfileAT.postSwitchFadeStartTime = millis(); + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.phase = MIXERAT_PHASE_POST_SWITCH_FADE; + isMixerTransitionMixing_requested = false; + return true; +} + +static bool updatePostSwitchFade(void) +{ + if (mixerProfileAT.phase != MIXERAT_PHASE_POST_SWITCH_FADE) { + return true; + } + + if (mixerProfileAT.postSwitchFadeDurationMs == 0) { + mixerProfileAT.postSwitchFadeProgress = 1.0f; + } else { + const uint32_t elapsedMs = millis() - mixerProfileAT.postSwitchFadeStartTime; + mixerProfileAT.postSwitchFadeProgress = constrainf((float)elapsedMs / (float)mixerProfileAT.postSwitchFadeDurationMs, 0.0f, 1.0f); + } + + const float remainingScale = mixerProfileAT.postSwitchFadeInitialScale * (1.0f - mixerProfileAT.postSwitchFadeProgress); + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = remainingScale; + mixerProfileAT.mcAuthorityScale = 0.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; + mixerProfileAT.blendToFw = 1.0f; + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + mixerProfileAT.pusherScale = remainingScale; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 0.0f; + mixerProfileAT.blendToFw = 0.0f; + } + + if (mixerProfileAT.postSwitchFadeProgress < 1.0f) { + return false; + } + + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + return true; +} + static uint16_t getAirspeedThresholdForDirection(const mixerProfileATDirection_e direction) { if (direction == MIXERAT_DIRECTION_TO_FW) { @@ -490,6 +623,10 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) { reprocessState=false; if (required_action == MIXERAT_REQUEST_ABORT) { + if (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE && mixerProfileAT.hotSwitchDone) { + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + return true; + } abortTransition(false); return true; } @@ -523,15 +660,18 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) isMixerTransitionMixing_requested = false; mixerProfileAT.progress = 1.0f; updateTransitionScales(); + preparePostSwitchFade(nextMixerProfileIndex); if (!outputProfileHotSwitch(nextMixerProfileIndex)) { abortTransition(false); return true; } mixerProfileAT.hotSwitchDone = true; - mixerProfileAT.phase = MIXERAT_PHASE_IDLE; - mixerProfileAT.request = MIXERAT_REQUEST_NONE; - mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; - reprocessState = true; + if (!startPostSwitchFade()) { + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + } + return true; } else if (mixerProfileAT.usedAirspeed && currentMixerConfig.vtolTransitionAirspeedTimeoutMs > 0 && (millis() - mixerProfileAT.transitionStartTime) >= currentMixerConfig.vtolTransitionAirspeedTimeoutMs) { @@ -542,6 +682,10 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) updateTransitionScales(); return false; break; + case MIXERAT_PHASE_POST_SWITCH_FADE: + isMixerTransitionMixing_requested = false; + updatePostSwitchFade(); + return true; default: break; } @@ -652,6 +796,11 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) mixerAT_inuse = false; } + if (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE) { + mixerATUpdateState(MIXERAT_REQUEST_NONE); + mixerAT_inuse = mixerATIsActive(); + } + if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) { if (mixerProfileModePresent && !transitionControllerOwnsProfileSwitch && !fwToMcProtectionOwnsProfileSwitch) { @@ -736,21 +885,29 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) (FLIGHT_MODE(FAILSAFE_MODE) ? 1U << 17 : 0U) | (manualControllerEnabled ? 1U << 18 : 0U) | (IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) ? 1U << 19 : 0U) | - (manualTransitionSessionLatched ? 1U << 20 : 0U); - - const uint8_t targetInputMode = - (isMixerTransitionMixing && - mixerATIsActive() && - mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) ? - (FLIGHT_MODE(MANUAL_MODE) ? 1U : 2U) : - 0U; + (manualTransitionSessionLatched ? 1U << 20 : 0U) | + (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE ? 1U << 29 : 0U); + + uint8_t targetInputMode = 0U; + if (isMixerTransitionMixing && mixerATIsActive()) { + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + targetInputMode = FLIGHT_MODE(MANUAL_MODE) ? 1U : 2U; + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + targetInputMode = 3U; + } + } const uint16_t progressScaled = lrintf(constrainf(mixerProfileAT.progress, 0.0f, 1.0f) * 1000.0f); const uint16_t handoffScaled = lrintf(constrainf(mixerProfileAT.handoffScalingProgress, 0.0f, 1.0f) * 1000.0f); const uint16_t motorRampScaled = lrintf(constrainf(mixerProfileAT.motorRampProgress, 0.0f, 1.0f) * 1000.0f); + const uint16_t postFadeScaled = lrintf(constrainf(mixerProfileAT.postSwitchFadeProgress, 0.0f, 1.0f) * 1000.0f); const uint16_t pusherScaled = lrintf(constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f) * 1000.0f); const uint16_t liftScaled = lrintf(constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) * 1000.0f); const uint16_t mcAuthorityScaled = lrintf(constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f) * 1000.0f); const uint16_t fwAuthorityScaled = lrintf(constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f) * 1000.0f); + const uint32_t packedProgress = + ((uint32_t)MIN(handoffScaled, 1000) & 0x3FFU) | + (((uint32_t)MIN(motorRampScaled, 1000) & 0x3FFU) << 10) | + (((uint32_t)MIN(postFadeScaled, 1000) & 0x3FFU) << 20); transitionDebugFlags |= (((uint32_t)currentMixerConfig.platformType & 0xFU) << 21); transitionDebugFlags |= (((uint32_t)pidIndexGetType(PID_ROLL) & 0x3U) << 25); @@ -764,7 +921,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) // [4] pusherScale x1000 // [5] liftScale x1000 // [6] mcAuthorityScale x1000 | (fwAuthorityScale x1000 << 16) - // [7] handoffProgress x1000 | (motorRampProgress x1000 << 16) + // [7] handoffProgress 10-bit | motorRampProgress 10-bit | postSwitchFadeProgress 10-bit DEBUG_SET(DEBUG_VTOL_TRANSITION, 0, mixerProfileAT.phase); DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, (int32_t)(((uint32_t)mixerProfileAT.request & 0xFFU) | (((uint32_t)mixerProfileAT.direction & 0xFFU) << 8))); DEBUG_SET(DEBUG_VTOL_TRANSITION, 2, (int32_t)transitionDebugFlags); @@ -772,9 +929,9 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_VTOL_TRANSITION, 4, pusherScaled); DEBUG_SET(DEBUG_VTOL_TRANSITION, 5, liftScaled); DEBUG_SET(DEBUG_VTOL_TRANSITION, 6, (int32_t)((uint32_t)mcAuthorityScaled | ((uint32_t)fwAuthorityScaled << 16))); - DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, (int32_t)((uint32_t)handoffScaled | ((uint32_t)motorRampScaled << 16))); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, (int32_t)packedProgress); - if (!isMixerTransitionMixing) { + if (!isMixerTransitionMixing && !mixerATIsActive()) { resetTransitionScales(); } #else @@ -859,6 +1016,31 @@ float mixerATGetBlendToFw(void) #endif } +#ifdef USE_AUTO_TRANSITION +bool mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output) +{ + if (mixerProfileAT.phase != MIXERAT_PHASE_POST_SWITCH_FADE || + motorIndex >= MAX_SUPPORTED_MOTORS || + (mixerProfileAT.postSwitchFadeMotorMask & (1U << motorIndex)) == 0) { + return false; + } + + const float holdScale = 1.0f - constrainf(mixerProfileAT.postSwitchFadeProgress, 0.0f, 1.0f); + const int16_t capturedOutput = mixerProfileAT.postSwitchFadeMotorOutput[motorIndex]; + const bool fadeToCurrentOutput = (mixerProfileAT.postSwitchFadeToCurrentMotorMask & (1U << motorIndex)) != 0; + const int16_t targetOutput = fadeToCurrentOutput ? currentOutput : idleOutput; + const int32_t fadedOutput = lrintf(targetOutput + (capturedOutput - targetOutput) * holdScale); + + *output = constrain(fadedOutput, MIN(targetOutput, capturedOutput), MAX(targetOutput, capturedOutput)); + return true; +} + +float mixerATGetPostSwitchFadeProgress(void) +{ + return constrainf(mixerProfileAT.postSwitchFadeProgress, 0.0f, 1.0f); +} +#endif + bool isMixerProfile2ModeReportedActive(void) { #if (MAX_MIXER_PROFILE_COUNT > 1) diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 5d1a4da3f93..e10640b5d60 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -62,6 +62,9 @@ typedef enum { MIXERAT_PHASE_IDLE, MIXERAT_PHASE_TRANSITION_INITIALIZE, MIXERAT_PHASE_TRANSITIONING, +#ifdef USE_AUTO_TRANSITION + MIXERAT_PHASE_POST_SWITCH_FADE, +#endif #ifndef USE_AUTO_TRANSITION MIXERAT_PHASE_DONE, #endif @@ -86,6 +89,13 @@ typedef struct mixerProfileAT_s { float liftScale; float mcAuthorityScale; float fwAuthorityScale; + float postSwitchFadeProgress; + float postSwitchFadeInitialScale; + uint16_t postSwitchFadeMotorMask; + uint16_t postSwitchFadeToCurrentMotorMask; + uint16_t postSwitchFadeDurationMs; + uint16_t postSwitchFadeMotorOutput[MAX_SUPPORTED_MOTORS]; + timeMs_t postSwitchFadeStartTime; timeMs_t transitionStartTime; #else bool transitionInputMixing; @@ -105,6 +115,10 @@ float mixerATGetLiftScale(void); float mixerATGetMcAuthorityScale(void); float mixerATGetFwAuthorityScale(void); float mixerATGetBlendToFw(void); +#ifdef USE_AUTO_TRANSITION +bool mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output); +float mixerATGetPostSwitchFadeProgress(void); +#endif bool isMixerProfile2ModeReportedActive(void); bool isMixerTransitionModeReportedActive(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c022c88e013..8de52eb6e52 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -128,6 +128,8 @@ typedef struct { float kI; float kD; float kFF; + float kCD; + float kT; float gyroRate; float rateTarget; float errorGyroIf; @@ -138,6 +140,7 @@ typedef struct { filter_t dtermLpfState; float previousRateTarget; float previousRateGyro; + pt3Filter_t rateTargetFilter; #ifdef USE_D_BOOST pt1Filter_t dBoostLpf; biquadFilter_t dBoostGyroLpf; @@ -184,6 +187,7 @@ static EXTENDED_FASTRAM pt1Filter_t autoTransitionTargetTpaFilter; static EXTENDED_FASTRAM filterApplyFnPtr autoTransitionTargetDTermLpfFilterApplyFn; static EXTENDED_FASTRAM uint8_t autoTransitionTargetYawLpfHz; static EXTENDED_FASTRAM int8_t autoTransitionTargetControlProfileIndex = -1; +static EXTENDED_FASTRAM bool autoTransitionTargetFixedWing = false; #endif #ifdef USE_ANTIGRAVITY @@ -495,18 +499,34 @@ static void resetAutoTransitionTargetPidState(void) memset(&autoTransitionTargetHeadingHoldRateFilter, 0, sizeof(autoTransitionTargetHeadingHoldRateFilter)); memset(&autoTransitionTargetTpaFilter, 0, sizeof(autoTransitionTargetTpaFilter)); autoTransitionTargetControlProfileIndex = -1; + autoTransitionTargetFixedWing = false; autoTransitionTargetDTermLpfFilterApplyFn = (filterApplyFnPtr)nullFilterApply; autoTransitionTargetYawLpfHz = 0; } static bool isAutoTransitionTargetPidActive(void) { - return isMixerTransitionMixing && - mixerATIsActive() && - mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && - isMultirotorTypePlatform(currentMixerConfig.platformType) && - nextMixerProfileIndex >= 0 && - nextMixerProfileIndex < MAX_PROFILE_COUNT && + if (!(isMixerTransitionMixing && + mixerATIsActive() && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT)) { + return false; + } + + const flyingPlatformType_e targetPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; + + return (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && + isMultirotorTypePlatform(currentMixerConfig.platformType) && + targetPlatformType == PLATFORM_AIRPLANE) || + (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && + currentMixerConfig.platformType == PLATFORM_AIRPLANE && + isMultirotorTypePlatform(targetPlatformType)); +} + +static bool isAutoTransitionTargetFixedWing(void) +{ + return nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT && mixerConfigByIndex(nextMixerProfileIndex)->platformType == PLATFORM_AIRPLANE; } @@ -544,13 +564,15 @@ static const pidProfile_t *getAutoTransitionTargetPidProfile(uint8_t profileInde return pidProfile(); } -static void initAutoTransitionTargetPidState(const uint8_t profileIndex, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile) +static void initAutoTransitionTargetPidState(const uint8_t profileIndex, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, const bool targetIsFixedWing) { const uint32_t refreshRate = getLooptime(); + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; resetAutoTransitionTargetPidState(); autoTransitionTargetControlProfileIndex = profileIndex; + autoTransitionTargetFixedWing = targetIsFixedWing; autoTransitionTargetYawLpfHz = targetPidProfile->yaw_lpf_hz; assignFilterApplyFn(targetPidProfile->dterm_lpf_type, targetPidProfile->dterm_lpf_hz, &autoTransitionTargetDTermLpfFilterApplyFn); @@ -569,7 +591,8 @@ static void initAutoTransitionTargetPidState(const uint8_t profileIndex, const c pt1FilterSetCutoff(&autoTransitionTargetPidState[axis].dBoostLpf, D_BOOST_LPF_HZ); #endif - pt1FilterSetCutoff(&autoTransitionTargetPidState[axis].angleFilterState, targetPidProfile->bank_fw.pid[PID_LEVEL].I); + pt1FilterSetCutoff(&autoTransitionTargetPidState[axis].angleFilterState, targetPidBank->pid[PID_LEVEL].I); + pt3FilterInit(&autoTransitionTargetPidState[axis].rateTargetFilter, pt3FilterGain(targetPidProfile->controlDerivativeLpfHz, US2S(refreshRate))); if (axis == FD_YAW && autoTransitionTargetYawLpfHz) { pt1FilterSetCutoff(&autoTransitionTargetPidState[axis].ptermLpfState, autoTransitionTargetYawLpfHz); @@ -656,34 +679,71 @@ static float calculateAutoTransitionTargetTPAFactor(const controlConfig_t *contr return tpaFactor; } -static void updateAutoTransitionTargetPIDCoefficients(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile) +static float calculateAutoTransitionTargetMultirotorTPAFactor(const controlConfig_t *controlProfile) +{ + const uint16_t throttle = rcCommand[THROTTLE]; + + if (controlProfile->throttle.dynPID == 0 || throttle < controlProfile->throttle.pa_breakpoint) { + return 1.0f; + } + + if (throttle < getMaxThrottle()) { + return (100 - (uint16_t)controlProfile->throttle.dynPID * (throttle - controlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - controlProfile->throttle.pa_breakpoint)) / 100.0f; + } + + return (100 - constrain(controlProfile->throttle.dynPID, 0, 100)) / 100.0f; +} + +static void updateAutoTransitionTargetPIDCoefficients(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, const bool targetIsFixedWing) { float tpaFactor = 1.0f; float iTermFactor = 1.0f; + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; - if (controlProfile->throttle.apa_pow > 0 && - pitotGetValidForAirspeed()) { - tpaFactor = calculateAutoTransitionTargetAirspeedTPAFactor(targetPidProfile, controlProfile); - iTermFactor = calculateAutoTransitionTargetAirspeedITermFactor(targetPidProfile, controlProfile); + if (targetIsFixedWing) { + if (controlProfile->throttle.apa_pow > 0 && + pitotGetValidForAirspeed()) { + tpaFactor = calculateAutoTransitionTargetAirspeedTPAFactor(targetPidProfile, controlProfile); + iTermFactor = calculateAutoTransitionTargetAirspeedITermFactor(targetPidProfile, controlProfile); + } else { + tpaFactor = calculateAutoTransitionTargetTPAFactor(controlProfile); + iTermFactor = tpaFactor; + } } else { - tpaFactor = calculateAutoTransitionTargetTPAFactor(controlProfile); - iTermFactor = tpaFactor; + tpaFactor = calculateAutoTransitionTargetMultirotorTPAFactor(controlProfile); + iTermFactor = 1.0f; } for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { - autoTransitionTargetPidState[axis].kP = targetPidProfile->bank_fw.pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; - autoTransitionTargetPidState[axis].kI = targetPidProfile->bank_fw.pid[axis].I / FP_PID_RATE_I_MULTIPLIER * iTermFactor; - autoTransitionTargetPidState[axis].kD = targetPidProfile->bank_fw.pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor; - autoTransitionTargetPidState[axis].kFF = targetPidProfile->bank_fw.pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; + const float axisTPA = (!targetIsFixedWing && axis == FD_YAW && !controlProfile->throttle.dynPID_on_YAW) ? 1.0f : tpaFactor; + + autoTransitionTargetPidState[axis].kP = targetPidBank->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA; + autoTransitionTargetPidState[axis].kI = targetPidBank->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * iTermFactor; + autoTransitionTargetPidState[axis].kD = targetPidBank->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA; + + if (targetIsFixedWing) { + autoTransitionTargetPidState[axis].kFF = targetPidBank->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; + autoTransitionTargetPidState[axis].kCD = 0.0f; + autoTransitionTargetPidState[axis].kT = 0.0f; + } else { + autoTransitionTargetPidState[axis].kFF = 0.0f; + autoTransitionTargetPidState[axis].kCD = (targetPidBank->pid[axis].FF / FP_PID_RATE_D_FF_MULTIPLIER * axisTPA) / (getLooptime() * 0.000001f); + if (targetPidBank->pid[axis].P != 0 && targetPidBank->pid[axis].I != 0) { + autoTransitionTargetPidState[axis].kT = 2.0f / ((autoTransitionTargetPidState[axis].kP / autoTransitionTargetPidState[axis].kI) + (autoTransitionTargetPidState[axis].kD / autoTransitionTargetPidState[axis].kP)); + } else { + autoTransitionTargetPidState[axis].kT = 0.0f; + } + } } } -static float calcAutoTransitionTargetHorizonRateMagnitude(const pidProfile_t *targetPidProfile) +static float calcAutoTransitionTargetHorizonRateMagnitude(const pidProfile_t *targetPidProfile, const bool targetIsFixedWing) { const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL)); const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH)); const float mostDeflectedStickPos = constrain(MAX(stickPosAil, stickPosEle), 0, 500) / 500.0f; - const float modeTransitionStickPos = constrain(targetPidProfile->bank_fw.pid[PID_LEVEL].D, 0, 100) / 100.0f; + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; + const float modeTransitionStickPos = constrain(targetPidBank->pid[PID_LEVEL].D, 0, 100) / 100.0f; if (modeTransitionStickPos <= 0.0f) { return 1.0f; @@ -718,7 +778,7 @@ static uint8_t getAutoTransitionTargetHeadingHoldState(const pidProfile_t *targe return HEADING_HOLD_UPDATE_HEADING; } -static float pidAutoTransitionTargetHeadingHold(const pidProfile_t *targetPidProfile, float dT) +static float pidAutoTransitionTargetHeadingHold(const pidProfile_t *targetPidProfile, const bool targetIsFixedWing, float dT) { int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget; @@ -728,24 +788,32 @@ static float pidAutoTransitionTargetHeadingHold(const pidProfile_t *targetPidPro error += 360; } - float headingHoldRate = error * targetPidProfile->bank_fw.pid[PID_HEADING].P / 30.0f; + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; + float headingHoldRate = error * targetPidBank->pid[PID_HEADING].P / 30.0f; headingHoldRate = constrainf(headingHoldRate, -targetPidProfile->heading_hold_rate_limit, targetPidProfile->heading_hold_rate_limit); headingHoldRate = pt1FilterApply3(&autoTransitionTargetHeadingHoldRateFilter, headingHoldRate, dT); return headingHoldRate; } -static float computeAutoTransitionTargetPidLevelTarget(const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis) +static float computeAutoTransitionTargetPidLevelTarget(const pidProfile_t *targetPidProfile, const bool targetIsFixedWing, flight_dynamics_index_t axis) { + uint16_t maxBankAngle = targetPidProfile->max_angle_inclination[axis]; + if (!targetIsFixedWing && + navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI && + isAdjustingPosition()) { + maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle); + } + float angleTarget; #ifdef USE_PROGRAMMING_FRAMEWORK - angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), targetPidProfile->max_angle_inclination[axis]); + angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), maxBankAngle); #else - angleTarget = pidRcCommandToAngle(rcCommand[axis], targetPidProfile->max_angle_inclination[axis]); + angleTarget = pidRcCommandToAngle(rcCommand[axis], maxBankAngle); #endif - if (axis == FD_PITCH) { + if (targetIsFixedWing && axis == FD_PITCH) { #ifdef USE_FW_AUTOLAND if (FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { #else @@ -771,13 +839,15 @@ static void pidAutoTransitionTargetLevel( autoTransitionTargetPidState_t *pidState, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, + const bool targetIsFixedWing, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT) { float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; - if (FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) { + if (targetIsFixedWing && FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) { angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)); if (!angleErrorDeg) { pidState->errorGyroIf = 0.0f; @@ -786,12 +856,12 @@ static void pidAutoTransitionTargetLevel( } float angleRateTarget = constrainf( - angleErrorDeg * (targetPidProfile->bank_fw.pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), + angleErrorDeg * (targetPidBank->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -controlProfile->stabilized.rates[axis] * 10.0f, controlProfile->stabilized.rates[axis] * 10.0f ); - if (targetPidProfile->bank_fw.pid[PID_LEVEL].I) { + if (targetPidBank->pid[PID_LEVEL].I) { angleRateTarget = pt1FilterApply3(&pidState->angleFilterState, angleRateTarget, dT); } @@ -819,6 +889,7 @@ static void pidAutoTransitionTargetTurnAssistant( autoTransitionTargetPidState_t *pidState, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, + const bool targetIsFixedWing, float bankAngleTarget, float pitchAngleTarget) { @@ -826,23 +897,27 @@ static void pidAutoTransitionTargetTurnAssistant( targetRates.x = 0.0f; targetRates.y = 0.0f; - if (calculateCosTiltAngle() < 0.173648f) { - return; - } + if (targetIsFixedWing) { + if (calculateCosTiltAngle() < 0.173648f) { + return; + } #if defined(USE_PITOT) - float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) && pitotIsHealthy() ? getAirspeedEstimate() : targetPidProfile->fixedWingReferenceAirspeed; + float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) && pitotIsHealthy() ? getAirspeedEstimate() : targetPidProfile->fixedWingReferenceAirspeed; #else - float airspeedForCoordinatedTurn = targetPidProfile->fixedWingReferenceAirspeed; + float airspeedForCoordinatedTurn = targetPidProfile->fixedWingReferenceAirspeed; #endif - airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300.0f, 6000.0f); - bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60)); + airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300.0f, 6000.0f); + bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60)); - const float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget)); - const float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor; + const float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget)); + const float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor; - targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); + targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); + } else { + targetRates.z = pidState[YAW].rateTarget; + } imuTransformVectorEarthToBody(&targetRates); @@ -856,14 +931,23 @@ static void pidAutoTransitionTargetTurnAssistant( -controlProfile->stabilized.rates[PITCH] * 10.0f, controlProfile->stabilized.rates[PITCH] * 10.0f ); - pidState[YAW].rateTarget = constrainf( - pidState[YAW].rateTarget + targetRates.z * targetPidProfile->fixedWingCoordinatedYawGain, - -controlProfile->stabilized.rates[YAW] * 10.0f, - controlProfile->stabilized.rates[YAW] * 10.0f - ); + + if (targetIsFixedWing) { + pidState[YAW].rateTarget = constrainf( + pidState[YAW].rateTarget + targetRates.z * targetPidProfile->fixedWingCoordinatedYawGain, + -controlProfile->stabilized.rates[YAW] * 10.0f, + controlProfile->stabilized.rates[YAW] * 10.0f + ); + } else { + pidState[YAW].rateTarget = constrainf( + targetRates.z, + -controlProfile->stabilized.rates[YAW] * 10.0f, + controlProfile->stabilized.rates[YAW] * 10.0f + ); + } } -static void updateAutoTransitionTargetRateTargets(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, float dT) +static void updateAutoTransitionTargetRateTargets(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, const bool targetIsFixedWing, float dT) { const uint8_t headingHoldState = getAutoTransitionTargetHeadingHoldState(targetPidProfile); @@ -871,7 +955,7 @@ static void updateAutoTransitionTargetRateTargets(const controlConfig_t *control float rateTarget; if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) { - rateTarget = pidAutoTransitionTargetHeadingHold(targetPidProfile, dT); + rateTarget = pidAutoTransitionTargetHeadingHold(targetPidProfile, targetIsFixedWing, dT); } else { #ifdef USE_PROGRAMMING_FRAMEWORK rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), controlProfile->stabilized.rates[axis]); @@ -883,27 +967,28 @@ static void updateAutoTransitionTargetRateTargets(const controlConfig_t *control autoTransitionTargetPidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); } - const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcAutoTransitionTargetHorizonRateMagnitude(targetPidProfile) : 0.0f; + const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcAutoTransitionTargetHorizonRateMagnitude(targetPidProfile, targetIsFixedWing) : 0.0f; for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) { - float angleTarget = getFlightAxisAngleOverride(axis, computeAutoTransitionTargetPidLevelTarget(targetPidProfile, axis)); + float angleTarget = getFlightAxisAngleOverride(axis, computeAutoTransitionTargetPidLevelTarget(targetPidProfile, targetIsFixedWing, axis)); if (STATE(TAILSITTER) && isMixerTransitionMixing && axis == FD_PITCH) { angleTarget += DEGREES_TO_DECIDEGREES(45); } - // Preview the target fixed-wing angle controller using the current transition stick command. - // Airplane angle-hold target memory is not duplicated here, so this remains a close preview - // rather than a bit-for-bit copy of the post-switch controller state. - pidAutoTransitionTargetLevel(angleTarget, &autoTransitionTargetPidState[axis], controlProfile, targetPidProfile, axis, horizonRateMagnitude, dT); + // Preview the target profile angle controller using the current + // transition stick command. Angle-hold target memory is not + // duplicated here, so this remains a close preview rather than a + // bit-for-bit copy of the post-switch controller state. + pidAutoTransitionTargetLevel(angleTarget, &autoTransitionTargetPidState[axis], controlProfile, targetPidProfile, targetIsFixedWing, axis, horizonRateMagnitude, dT); } } if (FLIGHT_MODE(TURN_ASSISTANT) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { const float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], targetPidProfile->max_angle_inclination[FD_ROLL])); const float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], targetPidProfile->max_angle_inclination[FD_PITCH])); - pidAutoTransitionTargetTurnAssistant(autoTransitionTargetPidState, controlProfile, targetPidProfile, bankAngleTarget, pitchAngleTarget); + pidAutoTransitionTargetTurnAssistant(autoTransitionTargetPidState, controlProfile, targetPidProfile, targetIsFixedWing, bankAngleTarget, pitchAngleTarget); } for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -1034,6 +1119,42 @@ static int16_t applyAutoTransitionTargetFixedWingRateController(autoTransitionTa return constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -limit, +limit); } +static int16_t applyAutoTransitionTargetMulticopterRateController(autoTransitionTargetPidState_t *pidState, const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis, const bool itermLimitActive, float dT, float dT_inv) +{ + const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); + const float rateError = rateTarget - pidState->gyroRate; + const float newPTerm = autoTransitionTargetPTermProcess(pidState, axis, rateError, dT); + const float newDTerm = autoTransitionTargetDTermProcess(pidState, targetPidProfile, rateTarget, dT, dT_inv); + + const float rateTargetDelta = rateTarget - pidState->previousRateTarget; + const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta); + const float newCDTerm = rateTargetDeltaFiltered * pidState->kCD; + + const uint16_t limit = 500; + const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; + const float newOutputLimited = constrainf(newOutput, -limit, +limit); + + /* + * Keep this preview independent from the active controller state. Reusing + * the global I-term relax filters here would make target-profile preview + * change the current-profile controller history during transition. + */ + pidState->errorGyroIf += (rateError * pidState->kI * antiWindupScaler * dT) + + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + + if (targetPidProfile->pidItermLimitPercent != 0) { + const float itermLimit = limit * targetPidProfile->pidItermLimitPercent * 0.01f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); + } + + applyAutoTransitionTargetItermLimiting(pidState, itermLimitActive); + + pidState->previousRateTarget = rateTarget; + pidState->previousRateGyro = pidState->gyroRate; + + return newOutputLimited; +} + static void updateAutoTransitionTargetAxisPID(float dT) { if (!pidFiltersConfigured || !isAutoTransitionTargetPidActive()) { @@ -1044,15 +1165,18 @@ static void updateAutoTransitionTargetAxisPID(float dT) const uint8_t profileIndex = getAutoTransitionTargetControlProfileIndex(); const controlConfig_t *controlProfile = getAutoTransitionTargetControlProfile(profileIndex); const pidProfile_t *targetPidProfile = getAutoTransitionTargetPidProfile(profileIndex); + const bool targetIsFixedWing = isAutoTransitionTargetFixedWing(); - if (autoTransitionTargetControlProfileIndex != profileIndex) { - initAutoTransitionTargetPidState(profileIndex, controlProfile, targetPidProfile); + if (autoTransitionTargetControlProfileIndex != profileIndex || + autoTransitionTargetFixedWing != targetIsFixedWing) { + initAutoTransitionTargetPidState(profileIndex, controlProfile, targetPidProfile, targetIsFixedWing); } - updateAutoTransitionTargetPIDCoefficients(controlProfile, targetPidProfile); - updateAutoTransitionTargetRateTargets(controlProfile, targetPidProfile, dT); + updateAutoTransitionTargetPIDCoefficients(controlProfile, targetPidProfile, targetIsFixedWing); + updateAutoTransitionTargetRateTargets(controlProfile, targetPidProfile, targetIsFixedWing, dT); const float dT_inv = 1.0f / dT; + const bool itermLimitActive = checkAutoTransitionTargetItermLimitingActive(); for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { autoTransitionTargetPidState[axis].gyroRate = gyro.gyroADCf[axis]; @@ -1061,11 +1185,22 @@ static void updateAutoTransitionTargetAxisPID(float dT) autoTransitionTargetPidState[axis].gyroRate = applySmithPredictor(axis, &autoTransitionTargetPidState[axis].smithPredictor, autoTransitionTargetPidState[axis].gyroRate); #endif - const bool itermLimitActive = checkAutoTransitionTargetItermLimitingActive(); - const bool itermFreezeActive = checkAutoTransitionTargetItermFreezingActive(targetPidProfile, axis); + if (targetIsFixedWing) { + const bool itermFreezeActive = checkAutoTransitionTargetItermFreezingActive(targetPidProfile, axis); + autoTransitionTargetAxisPID[axis] = applyAutoTransitionTargetFixedWingRateController(&autoTransitionTargetPidState[axis], controlProfile, targetPidProfile, axis, itermLimitActive, itermFreezeActive, dT, dT_inv); + } else { + autoTransitionTargetAxisPID[axis] = applyAutoTransitionTargetMulticopterRateController(&autoTransitionTargetPidState[axis], targetPidProfile, axis, itermLimitActive, dT, dT_inv); + } + } +} - autoTransitionTargetAxisPID[axis] = applyAutoTransitionTargetFixedWingRateController(&autoTransitionTargetPidState[axis], controlProfile, targetPidProfile, axis, itermLimitActive, itermFreezeActive, dT, dT_inv); +int16_t getAutoTransitionTargetAxisPID(flight_dynamics_index_t axis) +{ + if (!isAutoTransitionTargetPidActive()) { + return 0; } + + return autoTransitionTargetAxisPID[axis]; } int16_t getAutoTransitionTargetStabilizedInput(flight_dynamics_index_t axis) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c61a17f9405..71049667f68 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -194,6 +194,7 @@ extern int16_t autoTransitionTargetAxisPID[]; extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_F[], axisPID_Setpoint[]; #ifdef USE_AUTO_TRANSITION int16_t getAutoTransitionTargetStabilizedInput(flight_dynamics_index_t axis); +int16_t getAutoTransitionTargetAxisPID(flight_dynamics_index_t axis); #endif void pidInit(void); From cec4676426cf13938aba4198fb3af2ce33ea45b8 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 9 Jun 2026 14:48:07 +0300 Subject: [PATCH 29/42] Fix VTOL auto-transition preview and fade edge cases --- src/main/flight/mixer.c | 30 +++++++++++++++++++++++------- src/main/flight/mixer_profile.c | 4 ++++ 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 459b4e085fa..3f5b317d4d2 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -546,8 +546,9 @@ void FAST_CODE mixTable(void) const bool currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); const int targetMixerProfileIndex = autoTransitionMotorMixing ? nextMixerProfileIndex : -1; const motorMixer_t *targetMotorMixer = autoTransitionMotorMixing ? mixerMotorMixersByIndex(targetMixerProfileIndex) : NULL; + const mixerConfig_t *targetMixerConfig = autoTransitionMotorMixing ? mixerConfigByIndex(targetMixerProfileIndex) : NULL; const bool targetProfileIsMultirotor = autoTransitionMotorMixing ? - isMultirotorTypePlatform(mixerConfigByIndex(targetMixerProfileIndex)->platformType) : + isMultirotorTypePlatform(targetMixerConfig->platformType) : false; int16_t targetInput[3] = { 0, 0, 0 }; @@ -566,9 +567,9 @@ void FAST_CODE mixTable(void) targetInput[YAW] = getAutoTransitionTargetAxisPID(FD_YAW); } - targetInput[ROLL] = targetInput[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f) * targetAuthorityScale; - targetInput[PITCH] = targetInput[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f) * targetAuthorityScale; - targetInput[YAW] = targetInput[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f) * targetAuthorityScale; + targetInput[ROLL] = targetInput[ROLL] * (targetMixerConfig->transition_PID_mmix_multiplier_roll / 1000.0f) * targetAuthorityScale; + targetInput[PITCH] = targetInput[PITCH] * (targetMixerConfig->transition_PID_mmix_multiplier_pitch / 1000.0f) * targetAuthorityScale; + targetInput[YAW] = targetInput[YAW] * (targetMixerConfig->transition_PID_mmix_multiplier_yaw / 1000.0f) * targetAuthorityScale; } #endif @@ -583,18 +584,33 @@ void FAST_CODE mixTable(void) const motorMixer_t *targetMixer = autoTransitionMotorMixing ? &targetMotorMixer[i] : NULL; const bool currentMotorActive = currentMixer[i].throttle > 0.0f; const bool targetMotorActive = targetMixer && targetMixer->throttle > 0.0f; - const int16_t activeRpyMix = currentMotorActive ? + const float activeRpyMix = currentMotorActive ? (input[PITCH] * currentMixer[i].pitch + input[ROLL] * currentMixer[i].roll + -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) : 0; - const int16_t targetRpyMix = targetMotorActive ? + const float targetRpyMix = targetMotorActive ? (targetInput[PITCH] * targetMixer->pitch + targetInput[ROLL] * targetMixer->roll + -motorYawMultiplier * targetInput[YAW] * targetMixer->yaw) : 0; + float sharedRpyNormalizer = 1.0f; - rpyMix[i] = (activeRpyMix + targetRpyMix) * mixerScale; + if (currentMotorActive && targetMotorActive) { + const float activeAuthorityScale = currentProfileIsMultirotor ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + const float targetAuthorityScale = targetProfileIsMultirotor ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + const float authorityScaleSum = activeAuthorityScale + targetAuthorityScale; + + if (authorityScaleSum > 1.0f) { + sharedRpyNormalizer = 1.0f / authorityScaleSum; + } + } + + rpyMix[i] = (activeRpyMix + targetRpyMix) * sharedRpyNormalizer * mixerScale; #else rpyMix[i] = (input[PITCH] * currentMixer[i].pitch + diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 82eaf0e49f7..b427078634f 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -624,7 +624,11 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) reprocessState=false; if (required_action == MIXERAT_REQUEST_ABORT) { if (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE && mixerProfileAT.hotSwitchDone) { + // Once the target profile is active, abort cannot safely undo + // the hot-switch. Keep fading the old propulsion output toward + // the safe post-switch destination instead of freezing it. mixerProfileAT.request = MIXERAT_REQUEST_NONE; + updatePostSwitchFade(); return true; } abortTransition(false); From 5140a6a42d26d8334608a0fce17196c9ed91b6b6 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Fri, 12 Jun 2026 14:12:26 +0300 Subject: [PATCH 30/42] Preserve VTOL transition behavior across profile hot-switches Extract pure VTOL transition helpers for request gating, scaling, hot-switch progress and servo input handling. Keep legacy manual transition sessions isolated from the new auto-transition controller, preserve legacy 0/500 transition servo endpoints, and keep the auto MC->FW servo source monotonic through hot-switch and fade paths. Add unit coverage for transition logic, policy and end-to-end scenarios, including profile index permutations, automated switch gating, dynamic scaling, airspeed/timer fallback, and pusher/shared-tilt handoff cases. --- src/main/flight/mixer_profile.c | 278 +++++------ src/main/flight/mixer_profile.h | 1 + src/main/flight/mixer_transition_logic.h | 339 +++++++++++++ src/main/flight/servos.c | 2 +- src/test/unit/CMakeLists.txt | 3 + .../unit/mixer_transition_logic_unittest.cc | 153 ++++++ .../unit/mixer_transition_policy_unittest.cc | 464 ++++++++++++++++++ .../mixer_transition_scenarios_unittest.cc | 359 ++++++++++++++ 8 files changed, 1432 insertions(+), 167 deletions(-) create mode 100644 src/main/flight/mixer_transition_logic.h create mode 100644 src/test/unit/mixer_transition_logic_unittest.cc create mode 100644 src/test/unit/mixer_transition_policy_unittest.cc create mode 100644 src/test/unit/mixer_transition_scenarios_unittest.cc diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index b427078634f..c2fa69eaa9a 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -12,6 +12,8 @@ #include "drivers/time.h" #include "common/maths.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" +#include "flight/mixer_transition_logic.h" #include "common/axis.h" #include "flight/pid.h" #include "flight/servos.h" @@ -43,8 +45,9 @@ int nextMixerProfileIndex; #ifdef USE_AUTO_TRANSITION static bool manualTransitionModeWasActive; static bool manualTransitionReadyForEdge = true; -static bool manualTransitionSessionLatched; +static mixerTransitionManualSessionMode_e manualTransitionSessionMode; static bool manualFwToMcProtectionLatched; +static int16_t mixerTransitionServoInput; #endif // Keep PG version split because USE_AUTO_TRANSITION changes the stored mixer profile layout only on >512 KB targets. @@ -219,44 +222,28 @@ static void setLegacyTransitionScales(void) mixerProfileAT.postSwitchFadeInitialScale = 0.0f; mixerProfileAT.postSwitchFadeMotorMask = 0; mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; -} - -static float blendScale(float from, float to, float progress) -{ - return from + (to - from) * constrainf(progress, 0.0f, 1.0f); + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); } static float getMotorRampProgress(void) { - if (!currentMixerConfig.vtolTransitionDynamicMixer) { - mixerProfileAT.motorRampProgress = 1.0f; - return 1.0f; - } - - if (currentMixerConfig.vtolTransitionScaleRampTimeMs <= 0) { - mixerProfileAT.motorRampProgress = 1.0f; - return 1.0f; - } - const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; - mixerProfileAT.motorRampProgress = constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); + mixerProfileAT.motorRampProgress = mixerTransitionComputeMotorRampProgress( + currentMixerConfig.vtolTransitionDynamicMixer, + currentMixerConfig.vtolTransitionScaleRampTimeMs, + elapsedMs); return mixerProfileAT.motorRampProgress; } static float getHandoffScalingProgress(void) { - if (!currentMixerConfig.vtolTransitionDynamicMixer) { - return 1.0f; - } - - if (mixerProfileAT.usedAirspeed) { - mixerProfileAT.handoffScalingProgress = constrainf(mixerProfileAT.progress, 0.0f, 1.0f); - return mixerProfileAT.handoffScalingProgress; - } - - // Preserve already-applied handoff scaling if pitot drops out mid-transition. - // Without trusted pitot, handoff returns to the normal transition timer/progress behavior. - mixerProfileAT.handoffScalingProgress = MAX(mixerProfileAT.handoffScalingProgress, constrainf(mixerProfileAT.progress, 0.0f, 1.0f)); + mixerProfileAT.handoffScalingProgress = mixerTransitionResolveHandoffProgress( + currentMixerConfig.vtolTransitionDynamicMixer, + mixerProfileAT.usedAirspeed, + mixerProfileAT.handoffScalingProgress, + mixerProfileAT.progress); return mixerProfileAT.handoffScalingProgress; } @@ -320,32 +307,21 @@ static void preparePostSwitchFade(const int targetProfileIndex) const motorMixer_t *targetMotorMixer = mixerMotorMixersByIndex(targetProfileIndex); const bool currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); const uint8_t count = getMotorCount(); + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + currentMixerConfig.vtolTransitionDynamicMixer, + currentMixerConfig.vtolTransitionScaleRampTimeMs, + mixerProfileAT.direction, + currentProfileIsMultirotor, + count, + currentMotorMixer, + targetMotorMixer); + + mixerProfileAT.postSwitchFadeMotorMask = fadeMask.motorMask; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = fadeMask.toCurrentMotorMask; for (uint8_t i = 0; i < count && i < MAX_SUPPORTED_MOTORS; i++) { - const bool currentMotorActive = currentMotorMixer[i].throttle > 0.0f; - const bool targetMotorActive = targetMotorMixer[i].throttle > 0.0f; - // Fade only propulsion outputs that disappear after the hot-switch. - // Shared tilt motors are handled by complementary throttle blending and - // must not receive an extra captured-output overlay. - const bool oldLiftMotor = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && - currentProfileIsMultirotor && - currentMotorActive && - !targetMotorActive; - const bool oldPusherMotor = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && - !currentProfileIsMultirotor && - currentMotorActive && - !targetMotorActive; - const bool targetFwPusherMotor = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && - currentProfileIsMultirotor && - !currentMotorActive && - targetMotorActive; - - if (oldLiftMotor || oldPusherMotor || targetFwPusherMotor) { + if (mixerProfileAT.postSwitchFadeMotorMask & (1U << i)) { mixerProfileAT.postSwitchFadeMotorOutput[i] = motor[i]; - mixerProfileAT.postSwitchFadeMotorMask |= (1U << i); - if (targetFwPusherMotor) { - mixerProfileAT.postSwitchFadeToCurrentMotorMask |= (1U << i); - } } } @@ -447,37 +423,20 @@ static bool shouldRequestManualFwToMcProtection(const bool manualControllerEnabl static void updateTransitionScales(void) { - if (!currentMixerConfig.vtolTransitionDynamicMixer) { - mixerProfileAT.blendToFw = 1.0f; - mixerProfileAT.pusherScale = 1.0f; - mixerProfileAT.liftScale = 1.0f; - mixerProfileAT.mcAuthorityScale = 1.0f; - mixerProfileAT.fwAuthorityScale = 1.0f; - return; - } - - const float liftFloor = constrainf(systemConfig()->vtolTransitionLiftMinPercent / 100.0f, 0.0f, 1.0f); - const float mcFloor = constrainf(systemConfig()->vtolTransitionMcAuthorityMinPercent / 100.0f, 0.0f, 1.0f); - const float fwFloor = constrainf(systemConfig()->vtolTransitionFwAuthorityMinPercent / 100.0f, 0.0f, 1.0f); - const float handoffProgress = getHandoffScalingProgress(); - const float motorRampProgress = getMotorRampProgress(); - - if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { - mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, motorRampProgress); - mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, handoffProgress); - mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, handoffProgress); - mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, handoffProgress); - } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { - // In FW->MC, propulsion handover must not wait for airspeed-derived - // handoff progress. Ramp down the pusher and ramp up lift / MC - // stabilisation by time so the aircraft can actually slow down. - mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, motorRampProgress); - mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, motorRampProgress); - mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, motorRampProgress); - mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, handoffProgress); - } - - mixerProfileAT.blendToFw = constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + currentMixerConfig.vtolTransitionDynamicMixer, + mixerProfileAT.direction, + systemConfig()->vtolTransitionLiftMinPercent / 100.0f, + systemConfig()->vtolTransitionMcAuthorityMinPercent / 100.0f, + systemConfig()->vtolTransitionFwAuthorityMinPercent / 100.0f, + getHandoffScalingProgress(), + getMotorRampProgress()); + + mixerProfileAT.blendToFw = scales.blendToFw; + mixerProfileAT.pusherScale = scales.pusherScale; + mixerProfileAT.liftScale = scales.liftScale; + mixerProfileAT.mcAuthorityScale = scales.mcAuthorityScale; + mixerProfileAT.fwAuthorityScale = scales.fwAuthorityScale; } static void abortTransition(const bool byAirspeedTimeout) @@ -503,44 +462,23 @@ static bool mixerATReadyForHotSwitch(const mixerProfileATRequest_e required_acti const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; const uint32_t transitionTimerMs = MAX(0, currentMixerConfig.switchTransitionTimer) * 100; float airspeedCmS = 0.0f; - - if (direction == MIXERAT_DIRECTION_NONE) { - mixerProfileAT.progress = 0.0f; - mixerProfileAT.usedAirspeed = false; - return false; - } - - if (airspeedThresholdCmS > 0 && hasTrustedPitotAirspeed(&airspeedCmS)) { - mixerProfileAT.usedAirspeed = true; - if (direction == MIXERAT_DIRECTION_TO_FW) { - mixerProfileAT.progress = constrainf(airspeedCmS / airspeedThresholdCmS, 0.0f, 1.0f); - return airspeedCmS >= airspeedThresholdCmS; - } - - if (!mixerProfileAT.transitionStartAirspeedCaptured) { - mixerProfileAT.transitionStartAirspeedCmS = airspeedCmS; - mixerProfileAT.transitionStartAirspeedCaptured = true; - } - - const float startAirspeed = mixerProfileAT.transitionStartAirspeedCmS; - const float thresholdAirspeed = airspeedThresholdCmS; - if (startAirspeed <= thresholdAirspeed) { - mixerProfileAT.progress = 1.0f; - } else { - mixerProfileAT.progress = constrainf((startAirspeed - airspeedCmS) / (startAirspeed - thresholdAirspeed), 0.0f, 1.0f); - } - - return airspeedCmS <= airspeedThresholdCmS; - } - - mixerProfileAT.usedAirspeed = false; - if (transitionTimerMs > 0) { - mixerProfileAT.progress = constrainf((float)elapsedMs / (float)transitionTimerMs, 0.0f, 1.0f); - } else { - mixerProfileAT.progress = 1.0f; - } - - return elapsedMs >= transitionTimerMs; + const bool trustedAirspeedAvailable = + airspeedThresholdCmS > 0 && hasTrustedPitotAirspeed(&airspeedCmS); + const mixerTransitionHotSwitchProgress_t hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + direction, + airspeedThresholdCmS, + trustedAirspeedAvailable, + airspeedCmS, + mixerProfileAT.transitionStartAirspeedCaptured, + mixerProfileAT.transitionStartAirspeedCmS, + elapsedMs, + transitionTimerMs); + + mixerProfileAT.usedAirspeed = hotSwitchProgress.usedAirspeed; + mixerProfileAT.transitionStartAirspeedCaptured = hotSwitchProgress.transitionStartAirspeedCaptured; + mixerProfileAT.transitionStartAirspeedCmS = hotSwitchProgress.transitionStartAirspeedCmS; + mixerProfileAT.progress = hotSwitchProgress.progress; + return hotSwitchProgress.readyForHotSwitch; } #endif @@ -564,42 +502,26 @@ static bool missionTransitionToMultirotorTypeConfigured(void) } #endif -bool checkMixerATRequired(mixerProfileATRequest_e required_action) +#ifdef USE_AUTO_TRANSITION +static bool isLegacyManualTransitionSessionActive(void) { - //return false if mixerAT condition is not required or setting is not valid - if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) - { - return false; - } - if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) - { - return false; - } + return manualTransitionSessionMode == MIXER_TRANSITION_MANUAL_SESSION_LEGACY; +} +#endif +bool checkMixerATRequired(mixerProfileATRequest_e required_action) +{ #ifdef USE_AUTO_TRANSITION - 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(); - - case MIXERAT_REQUEST_MANUAL_TO_FW: - return STATE(MULTIROTOR) && platformTypeConfigured(PLATFORM_AIRPLANE); - - case MIXERAT_REQUEST_MANUAL_TO_MC: - return STATE(AIRPLANE) && missionTransitionToMultirotorTypeConfigured(); - - default: - return false; - } + return mixerTransitionIsRequestAllowed( + required_action, + STATE(AIRPLANE), + STATE(MULTIROTOR), + isModeActivationConditionPresent(BOXMIXERPROFILE), + currentMixerConfig.automated_switch, + platformTypeConfigured(PLATFORM_AIRPLANE), + missionTransitionToMultirotorTypeConfigured()); #else + // Legacy 512 KB targets keep the original automated-switch behaviour. if(currentMixerConfig.automated_switch){ if ((required_action == MIXERAT_REQUEST_RTH) && STATE(MULTIROTOR)) { @@ -765,7 +687,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS); const bool missionActive = (navGetCurrentStateFlags() & NAV_AUTO_WP) != 0; const bool manualControllerConfigured = currentMixerConfig.manualVtolTransitionController && !missionActive; - bool manualControllerEnabled = manualControllerConfigured || manualTransitionSessionLatched; + bool manualControllerEnabled = mixerTransitionManualControllerEnabled(manualControllerConfigured, manualTransitionSessionMode); const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; const bool mixerProfileModePresent = isModeActivationConditionPresent(BOXMIXERPROFILE); const int requestedProfileIndex = IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1; @@ -777,25 +699,24 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) STATE(MULTIROTOR) && !requestedMultirotorProfile; - if (manualControllerConfigured && transitionModeRisingEdge) { - manualTransitionSessionLatched = true; - } + manualTransitionSessionMode = mixerTransitionUpdateManualSessionMode( + manualTransitionSessionMode, + transitionModeRisingEdge, + transitionModeFallingEdge, + manualControllerConfigured, + false); if (transitionModeRisingEdge) { manualFwToMcProtectionLatched = false; } - if (transitionModeFallingEdge) { - manualTransitionSessionLatched = false; - } - if (requestedMultirotorProfile || (!mixerAT_inuse && !STATE(MULTIROTOR))) { manualFwToMcProtectionLatched = false; } if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { abortTransition(false); - manualTransitionSessionLatched = false; + manualTransitionSessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; manualFwToMcProtectionLatched = false; mixerAT_inuse = false; } @@ -813,7 +734,9 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) } // Recompute after a potential direct profile hot-switch because this flag is per-mixer-profile. - manualControllerEnabled = (currentMixerConfig.manualVtolTransitionController && !missionActive) || manualTransitionSessionLatched; + manualControllerEnabled = mixerTransitionManualControllerEnabled( + currentMixerConfig.manualVtolTransitionController && !missionActive, + manualTransitionSessionMode); if (!mixerAT_inuse && shouldRequestManualFwToMcProtection(manualControllerEnabled) && @@ -836,7 +759,6 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) manualTransitionReadyForEdge = true; } else { if (!transitionModeActive) { - manualTransitionSessionLatched = false; manualTransitionReadyForEdge = true; if (!mixerAT_inuse) { isMixerTransitionMixing_requested = false; @@ -889,7 +811,8 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) (FLIGHT_MODE(FAILSAFE_MODE) ? 1U << 17 : 0U) | (manualControllerEnabled ? 1U << 18 : 0U) | (IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) ? 1U << 19 : 0U) | - (manualTransitionSessionLatched ? 1U << 20 : 0U) | + (manualTransitionSessionMode != MIXER_TRANSITION_MANUAL_SESSION_NONE ? 1U << 20 : 0U) | + (isLegacyManualTransitionSessionActive() ? 1U << 30 : 0U) | (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE ? 1U << 29 : 0U); uint8_t targetInputMode = 0U; @@ -936,6 +859,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, (int32_t)packedProgress); if (!isMixerTransitionMixing && !mixerATIsActive()) { + mixerTransitionServoInput = 0; resetTransitionScales(); } #else @@ -1020,6 +944,28 @@ float mixerATGetBlendToFw(void) #endif } +int16_t mixerATGetTransitionServoInput(void) +{ +#ifdef USE_AUTO_TRANSITION + const bool postSwitchFadeToFwActive = + mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW; + + mixerTransitionServoInput = mixerTransitionUpdateServoInput( + mixerTransitionServoInput, + isLegacyManualTransitionSessionActive(), + isMixerTransitionMixing, + mixerATIsActive(), + postSwitchFadeToFwActive, + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW, + mixerATGetBlendToFw()); + + return mixerTransitionServoInput; +#else + return isMixerTransitionMixing ? 500 : 0; +#endif +} + #ifdef USE_AUTO_TRANSITION bool mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output) { @@ -1063,7 +1009,7 @@ bool isMixerTransitionModeReportedActive(void) } // With manual auto-transition enabled (or session latched), treat RC as trigger only. - if (currentMixerConfig.manualVtolTransitionController || manualTransitionSessionLatched) { + if (mixerTransitionManualControllerEnabled(currentMixerConfig.manualVtolTransitionController, manualTransitionSessionMode)) { return false; } diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index e10640b5d60..0b72dd5116c 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -115,6 +115,7 @@ float mixerATGetLiftScale(void); float mixerATGetMcAuthorityScale(void); float mixerATGetFwAuthorityScale(void); float mixerATGetBlendToFw(void); +int16_t mixerATGetTransitionServoInput(void); #ifdef USE_AUTO_TRANSITION bool mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output); float mixerATGetPostSwitchFadeProgress(void); diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h new file mode 100644 index 00000000000..1037328434a --- /dev/null +++ b/src/main/flight/mixer_transition_logic.h @@ -0,0 +1,339 @@ +#pragma once + +#include +#include + +#include "flight/mixer_profile.h" + +typedef enum { + MIXER_TRANSITION_MANUAL_SESSION_NONE = 0, + MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + MIXER_TRANSITION_MANUAL_SESSION_AUTO, +} mixerTransitionManualSessionMode_e; + +#ifdef USE_AUTO_TRANSITION +typedef struct { + bool readyForHotSwitch; + bool usedAirspeed; + bool transitionStartAirspeedCaptured; + float transitionStartAirspeedCmS; + float progress; +} mixerTransitionHotSwitchProgress_t; + +typedef struct { + float pusherScale; + float liftScale; + float mcAuthorityScale; + float fwAuthorityScale; + float blendToFw; +} mixerTransitionScaleState_t; + +typedef struct { + uint16_t motorMask; + uint16_t toCurrentMotorMask; +} mixerTransitionPostSwitchFadeMask_t; +#endif + +static inline mixerTransitionManualSessionMode_e mixerTransitionUpdateManualSessionMode( + mixerTransitionManualSessionMode_e currentMode, + bool transitionModeRisingEdge, + bool transitionModeFallingEdge, + bool manualControllerConfigured, + bool clearSession) +{ + if (clearSession || transitionModeFallingEdge) { + return MIXER_TRANSITION_MANUAL_SESSION_NONE; + } + + if (transitionModeRisingEdge) { + return manualControllerConfigured ? + MIXER_TRANSITION_MANUAL_SESSION_AUTO : + MIXER_TRANSITION_MANUAL_SESSION_LEGACY; + } + + return currentMode; +} + +static inline float mixerTransitionClamp(float value, float low, float high) +{ + if (value < low) { + return low; + } + + if (value > high) { + return high; + } + + return value; +} + +static inline bool mixerTransitionManualControllerEnabled( + bool manualControllerConfigured, + mixerTransitionManualSessionMode_e sessionMode) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO || + (sessionMode != MIXER_TRANSITION_MANUAL_SESSION_LEGACY && manualControllerConfigured); +} + +static inline bool mixerTransitionIsRequestAllowed( + mixerProfileATRequest_e requiredAction, + bool stateAirplane, + bool stateMultirotor, + bool mixerProfileModePresent, + bool automatedSwitch, + bool targetProfileIsAirplane, + bool targetProfileIsMultirotor) +{ + if ((!stateAirplane && !stateMultirotor) || !mixerProfileModePresent) { + return false; + } + + switch (requiredAction) { + case MIXERAT_REQUEST_RTH: + return automatedSwitch && stateMultirotor && targetProfileIsAirplane; + + case MIXERAT_REQUEST_LAND: + return automatedSwitch && stateAirplane && targetProfileIsMultirotor; + +#ifdef USE_AUTO_TRANSITION + case MIXERAT_REQUEST_MISSION_TO_FW: + case MIXERAT_REQUEST_MANUAL_TO_FW: + return stateMultirotor && targetProfileIsAirplane; + + case MIXERAT_REQUEST_MISSION_TO_MC: + case MIXERAT_REQUEST_MANUAL_TO_MC: + return stateAirplane && targetProfileIsMultirotor; +#endif + + default: + return false; + } +} + +#ifdef USE_AUTO_TRANSITION +static inline float mixerTransitionComputeMotorRampProgress( + bool dynamicMixerEnabled, + uint16_t scaleRampTimeMs, + uint32_t elapsedMs) +{ + if (!dynamicMixerEnabled || scaleRampTimeMs == 0) { + return 1.0f; + } + + return mixerTransitionClamp((float)elapsedMs / (float)scaleRampTimeMs, 0.0f, 1.0f); +} + +static inline float mixerTransitionResolveHandoffProgress( + bool dynamicMixerEnabled, + bool usedAirspeed, + float previousHandoffProgress, + float rawProgress) +{ + const float clampedProgress = mixerTransitionClamp(rawProgress, 0.0f, 1.0f); + + if (!dynamicMixerEnabled) { + return 1.0f; + } + + if (usedAirspeed) { + return clampedProgress; + } + + return previousHandoffProgress > clampedProgress ? previousHandoffProgress : clampedProgress; +} + +static inline float mixerTransitionBlendScale(float from, float to, float progress) +{ + return from + (to - from) * mixerTransitionClamp(progress, 0.0f, 1.0f); +} + +static inline mixerTransitionScaleState_t mixerTransitionComputeScales( + bool dynamicMixerEnabled, + mixerProfileATDirection_e direction, + float liftFloor, + float mcFloor, + float fwFloor, + float handoffProgress, + float motorRampProgress) +{ + mixerTransitionScaleState_t scales = { + .pusherScale = 1.0f, + .liftScale = 1.0f, + .mcAuthorityScale = 1.0f, + .fwAuthorityScale = 1.0f, + .blendToFw = 1.0f, + }; + + if (!dynamicMixerEnabled) { + return scales; + } + + liftFloor = mixerTransitionClamp(liftFloor, 0.0f, 1.0f); + mcFloor = mixerTransitionClamp(mcFloor, 0.0f, 1.0f); + fwFloor = mixerTransitionClamp(fwFloor, 0.0f, 1.0f); + + if (direction == MIXERAT_DIRECTION_TO_FW) { + scales.pusherScale = mixerTransitionBlendScale(0.0f, 1.0f, motorRampProgress); + scales.liftScale = mixerTransitionBlendScale(1.0f, liftFloor, handoffProgress); + scales.mcAuthorityScale = mixerTransitionBlendScale(1.0f, mcFloor, handoffProgress); + scales.fwAuthorityScale = mixerTransitionBlendScale(fwFloor, 1.0f, handoffProgress); + } else if (direction == MIXERAT_DIRECTION_TO_MC) { + scales.pusherScale = mixerTransitionBlendScale(1.0f, 0.0f, motorRampProgress); + scales.liftScale = mixerTransitionBlendScale(liftFloor, 1.0f, motorRampProgress); + scales.mcAuthorityScale = mixerTransitionBlendScale(mcFloor, 1.0f, motorRampProgress); + scales.fwAuthorityScale = mixerTransitionBlendScale(1.0f, fwFloor, handoffProgress); + } + + scales.blendToFw = mixerTransitionClamp(scales.fwAuthorityScale, 0.0f, 1.0f); + return scales; +} + +static inline mixerTransitionHotSwitchProgress_t mixerTransitionEvaluateHotSwitch( + mixerProfileATDirection_e direction, + uint16_t airspeedThresholdCmS, + bool trustedAirspeedAvailable, + float currentAirspeedCmS, + bool transitionStartAirspeedCaptured, + float transitionStartAirspeedCmS, + uint32_t elapsedMs, + uint32_t transitionTimerMs) +{ + mixerTransitionHotSwitchProgress_t result = { + .readyForHotSwitch = false, + .usedAirspeed = false, + .transitionStartAirspeedCaptured = transitionStartAirspeedCaptured, + .transitionStartAirspeedCmS = transitionStartAirspeedCmS, + .progress = 0.0f, + }; + + if (direction == MIXERAT_DIRECTION_NONE) { + return result; + } + + if (airspeedThresholdCmS > 0 && trustedAirspeedAvailable) { + result.usedAirspeed = true; + + if (direction == MIXERAT_DIRECTION_TO_FW) { + result.progress = mixerTransitionClamp(currentAirspeedCmS / airspeedThresholdCmS, 0.0f, 1.0f); + result.readyForHotSwitch = currentAirspeedCmS >= airspeedThresholdCmS; + return result; + } + + if (!result.transitionStartAirspeedCaptured) { + result.transitionStartAirspeedCmS = currentAirspeedCmS; + result.transitionStartAirspeedCaptured = true; + } + + if (result.transitionStartAirspeedCmS <= airspeedThresholdCmS) { + result.progress = 1.0f; + } else { + result.progress = mixerTransitionClamp( + (result.transitionStartAirspeedCmS - currentAirspeedCmS) / + (result.transitionStartAirspeedCmS - airspeedThresholdCmS), + 0.0f, + 1.0f); + } + + result.readyForHotSwitch = currentAirspeedCmS <= airspeedThresholdCmS; + return result; + } + + result.progress = transitionTimerMs > 0 ? + mixerTransitionClamp((float)elapsedMs / (float)transitionTimerMs, 0.0f, 1.0f) : + 1.0f; + result.readyForHotSwitch = elapsedMs >= transitionTimerMs; + return result; +} + +static inline mixerTransitionPostSwitchFadeMask_t mixerTransitionComputePostSwitchFadeMask( + bool dynamicMixerEnabled, + uint16_t scaleRampTimeMs, + mixerProfileATDirection_e direction, + bool currentProfileIsMultirotor, + uint8_t motorCount, + const motorMixer_t *currentMotorMixer, + const motorMixer_t *targetMotorMixer) +{ + mixerTransitionPostSwitchFadeMask_t mask = { 0, 0 }; + + if (!dynamicMixerEnabled || + scaleRampTimeMs == 0 || + direction == MIXERAT_DIRECTION_NONE || + !currentMotorMixer || + !targetMotorMixer) { + return mask; + } + + for (uint8_t i = 0; i < motorCount && i < MAX_SUPPORTED_MOTORS; i++) { + const bool currentMotorActive = currentMotorMixer[i].throttle > 0.0f; + const bool targetMotorActive = targetMotorMixer[i].throttle > 0.0f; + const bool oldLiftMotor = direction == MIXERAT_DIRECTION_TO_FW && + currentProfileIsMultirotor && + currentMotorActive && + !targetMotorActive; + const bool oldPusherMotor = direction == MIXERAT_DIRECTION_TO_MC && + !currentProfileIsMultirotor && + currentMotorActive && + !targetMotorActive; + const bool targetFwPusherMotor = direction == MIXERAT_DIRECTION_TO_FW && + currentProfileIsMultirotor && + !currentMotorActive && + targetMotorActive; + + if (oldLiftMotor || oldPusherMotor || targetFwPusherMotor) { + mask.motorMask |= (1U << i); + if (targetFwPusherMotor) { + mask.toCurrentMotorMask |= (1U << i); + } + } + } + + return mask; +} +#endif + +static inline int16_t mixerTransitionBlendToServoInput(float blendToFw) +{ + if (blendToFw <= 0.0f) { + return 0; + } + + if (blendToFw >= 1.0f) { + return 500; + } + + return (int16_t)(blendToFw * 500.0f + 0.5f); +} + +static inline int16_t mixerTransitionUpdateServoInput( + int16_t previousInput, + bool legacyManualTransitionActive, + bool transitionMixingActive, + bool autoTransitionActive, + bool postSwitchFadeToFwActive, + bool transitionToFw, + float blendToFw) +{ + int16_t desiredInput = 0; + + if (legacyManualTransitionActive) { + desiredInput = transitionMixingActive ? 500 : 0; + } else if (postSwitchFadeToFwActive) { + desiredInput = 500; + } else if (transitionMixingActive) { + desiredInput = autoTransitionActive ? mixerTransitionBlendToServoInput(blendToFw) : 500; + } else if (autoTransitionActive && transitionToFw) { + desiredInput = mixerTransitionBlendToServoInput(blendToFw); + } + + // Only the new auto-transition MC->FW path needs the anti-reverse latch. + // Legacy mode must keep its original fixed 0/500 endpoint behaviour. + if (transitionToFw && + desiredInput < previousInput && + !legacyManualTransitionActive && + (transitionMixingActive || autoTransitionActive || postSwitchFadeToFwActive)) { + return previousInput; + } + + return desiredInput; +} diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 79e22260d97..1680eb87c86 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -467,7 +467,7 @@ void servoMixer(float dT) input[INPUT_STABILIZED_THROTTLE] = mixerThrottleCommand - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] - input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing ? lrintf(mixerATGetBlendToFw() * 500.0f) : 0; + input[INPUT_MIXER_TRANSITION] = mixerATGetTransitionServoInput(); input[INPUT_MIXER_SWITCH_HELPER] = 0; // no input, used to apply speed limit filter from previous servo rules // center the RC input value around the RC middle value diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 5b43b636a77..8aa20681d2c 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -69,6 +69,9 @@ set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TE set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c") set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER) +set_property(SOURCE mixer_transition_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE mixer_transition_policy_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE mixer_transition_scenarios_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) function(unit_test src) get_filename_component(basename ${src} NAME) diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc new file mode 100644 index 00000000000..f7f3478a921 --- /dev/null +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -0,0 +1,153 @@ +#include + +extern "C" { +#include "flight/mixer_transition_logic.h" +} + +TEST(MixerTransitionLogicTest, LegacyManualSessionStaysLegacyAfterHotSwitch) +{ + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + true, + false, + false, + false); + + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_LEGACY, sessionMode); + EXPECT_FALSE(mixerTransitionManualControllerEnabled(true, sessionMode)); + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + false, + true, + true, + false); + + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, sessionMode); +} + +TEST(MixerTransitionLogicTest, AutoManualSessionStaysAutoAcrossProfileChanges) +{ + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + true, + false, + true, + false); + + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, sessionMode); + EXPECT_TRUE(mixerTransitionManualControllerEnabled(false, sessionMode)); +} + +TEST(MixerTransitionLogicTest, LegacySessionIgnoresAutoControllerAfterProfileHotSwitch) +{ + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + true, + false, + false, + false); + + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_LEGACY, sessionMode); + EXPECT_FALSE(mixerTransitionManualControllerEnabled(true, sessionMode)); + EXPECT_EQ(500, mixerTransitionUpdateServoInput( + 0, + sessionMode == MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + true, + false, + false, + true, + 0.18f)); +} + +TEST(MixerTransitionLogicTest, LegacyServoInputMatchesPrePrFixedEndpoint) +{ + EXPECT_EQ(500, mixerTransitionUpdateServoInput( + 0, + true, + true, + false, + false, + true, + 0.12f)); + + EXPECT_EQ(0, mixerTransitionUpdateServoInput( + 500, + true, + false, + false, + false, + true, + 0.12f)); +} + +TEST(MixerTransitionLogicTest, AutoServoInputDoesNotMoveBackwardsDuringMcToFw) +{ + int16_t servoInput = 0; + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + true, + true, + false, + true, + 0.30f); + EXPECT_EQ(150, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + true, + true, + false, + true, + 0.62f); + EXPECT_EQ(310, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + true, + true, + false, + true, + 0.58f); + EXPECT_EQ(310, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + false, + true, + false, + true, + 0.20f); + EXPECT_EQ(310, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + false, + true, + true, + true, + 0.20f); + EXPECT_EQ(500, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + false, + false, + false, + false, + 0.0f); + EXPECT_EQ(0, servoInput); +} diff --git a/src/test/unit/mixer_transition_policy_unittest.cc b/src/test/unit/mixer_transition_policy_unittest.cc new file mode 100644 index 00000000000..4ff79f325b9 --- /dev/null +++ b/src/test/unit/mixer_transition_policy_unittest.cc @@ -0,0 +1,464 @@ +#include + +extern "C" { +#include "flight/mixer_transition_logic.h" +} + +TEST(MixerTransitionPolicyTest, RequestGatingWorksWhenFixedWingProfileIsIndexZero) +{ + const flyingPlatformType_e profileTypes[] = { + PLATFORM_AIRPLANE, + PLATFORM_TRICOPTER, + }; + const int targetFwIndex = 0; + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_FW, + false, + true, + true, + false, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_RTH, + false, + true, + true, + false, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_RTH, + false, + true, + true, + true, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); +} + +TEST(MixerTransitionPolicyTest, RequestGatingWorksWhenFixedWingProfileIsIndexOne) +{ + const flyingPlatformType_e profileTypes[] = { + PLATFORM_TRICOPTER, + PLATFORM_AIRPLANE, + }; + const int targetFwIndex = 1; + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MISSION_TO_FW, + false, + true, + true, + false, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MISSION_TO_FW, + false, + true, + false, + false, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); +} + +TEST(MixerTransitionPolicyTest, LandRequestNeedsAutomatedSwitchWhenMultirotorProfileIsIndexZero) +{ + const flyingPlatformType_e profileTypes[] = { + PLATFORM_TRICOPTER, + PLATFORM_AIRPLANE, + }; + const int targetMcIndex = 0; + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_LAND, + true, + false, + true, + false, + profileTypes[targetMcIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetMcIndex]))); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_LAND, + true, + false, + true, + true, + profileTypes[targetMcIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetMcIndex]))); +} + +TEST(MixerTransitionPolicyTest, LandRequestNeedsAutomatedSwitchWhenMultirotorProfileIsIndexOne) +{ + const flyingPlatformType_e profileTypes[] = { + PLATFORM_AIRPLANE, + PLATFORM_TRICOPTER, + }; + const int targetMcIndex = 1; + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_LAND, + true, + false, + true, + false, + profileTypes[targetMcIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetMcIndex]))); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_LAND, + true, + false, + true, + true, + profileTypes[targetMcIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetMcIndex]))); +} + +TEST(MixerTransitionPolicyTest, ManualRequestsNeedMixerProfileModeAndMatchingTargetType) +{ + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_FW, + false, + true, + false, + false, + true, + false)); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_FW, + false, + true, + true, + false, + false, + false)); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_MC, + true, + false, + true, + false, + true, + false)); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_MC, + true, + false, + true, + false, + false, + true)); +} + +TEST(MixerTransitionPolicyTest, DynamicScalingDisabledKeepsAllScalesAtFullValues) +{ + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + false, + MIXERAT_DIRECTION_TO_FW, + 0.20f, + 0.30f, + 0.40f, + 0.50f, + 0.25f); + + EXPECT_FLOAT_EQ(1.0f, scales.pusherScale); + EXPECT_FLOAT_EQ(1.0f, scales.liftScale); + EXPECT_FLOAT_EQ(1.0f, scales.mcAuthorityScale); + EXPECT_FLOAT_EQ(1.0f, scales.fwAuthorityScale); + EXPECT_FLOAT_EQ(1.0f, scales.blendToFw); +} + +TEST(MixerTransitionPolicyTest, McToFwDynamicScalingUsesHandoffAndRampProgress) +{ + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + true, + MIXERAT_DIRECTION_TO_FW, + 0.20f, + 0.30f, + 0.40f, + 0.50f, + 0.25f); + + EXPECT_FLOAT_EQ(0.25f, scales.pusherScale); + EXPECT_FLOAT_EQ(0.60f, scales.liftScale); + EXPECT_FLOAT_EQ(0.65f, scales.mcAuthorityScale); + EXPECT_FLOAT_EQ(0.70f, scales.fwAuthorityScale); + EXPECT_FLOAT_EQ(0.70f, scales.blendToFw); +} + +TEST(MixerTransitionPolicyTest, FwToMcDynamicScalingUsesTimerRampForLiftAndPusher) +{ + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + true, + MIXERAT_DIRECTION_TO_MC, + 0.20f, + 0.30f, + 0.40f, + 0.50f, + 0.25f); + + EXPECT_FLOAT_EQ(0.75f, scales.pusherScale); + EXPECT_FLOAT_EQ(0.40f, scales.liftScale); + EXPECT_FLOAT_EQ(0.475f, scales.mcAuthorityScale); + EXPECT_FLOAT_EQ(0.70f, scales.fwAuthorityScale); + EXPECT_FLOAT_EQ(0.70f, scales.blendToFw); +} + +TEST(MixerTransitionPolicyTest, HandoffProgressUsesAirspeedDirectlyWhenAvailable) +{ + EXPECT_FLOAT_EQ(0.35f, mixerTransitionResolveHandoffProgress(true, true, 0.80f, 0.35f)); +} + +TEST(MixerTransitionPolicyTest, HandoffProgressPreservesPreviousPeakWhenAirspeedDropsOut) +{ + EXPECT_FLOAT_EQ(0.62f, mixerTransitionResolveHandoffProgress(true, false, 0.62f, 0.41f)); + EXPECT_FLOAT_EQ(0.91f, mixerTransitionResolveHandoffProgress(true, false, 0.62f, 0.91f)); +} + +TEST(MixerTransitionPolicyTest, MotorRampProgressFallsBackToFullWhenDisabledOrZeroRamp) +{ + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeMotorRampProgress(false, 500, 100)); + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeMotorRampProgress(true, 0, 100)); + EXPECT_FLOAT_EQ(0.40f, mixerTransitionComputeMotorRampProgress(true, 500, 200)); +} + +TEST(MixerTransitionPolicyTest, DirectionNoneKeepsScalesAndHotSwitchIdle) +{ + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + true, + MIXERAT_DIRECTION_NONE, + 0.20f, + 0.30f, + 0.40f, + 0.50f, + 0.25f); + + EXPECT_FLOAT_EQ(1.0f, scales.pusherScale); + EXPECT_FLOAT_EQ(1.0f, scales.liftScale); + EXPECT_FLOAT_EQ(1.0f, scales.mcAuthorityScale); + EXPECT_FLOAT_EQ(1.0f, scales.fwAuthorityScale); + EXPECT_FLOAT_EQ(1.0f, scales.blendToFw); + + const mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_NONE, + 1500, + true, + 1800.0f, + false, + 0.0f, + 300, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_FALSE(progress.usedAirspeed); + EXPECT_FALSE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(0.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, HotSwitchFallsBackToTimerWhenAirspeedIsUnavailable) +{ + const mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1500, + false, + 0.0f, + false, + 0.0f, + 300, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_FALSE(progress.usedAirspeed); + EXPECT_FALSE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(0.30f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, HotSwitchUsesAirspeedWhenAvailableForMcToFw) +{ + const mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1500, + true, + 1800.0f, + false, + 0.0f, + 0, + 1000); + + EXPECT_TRUE(progress.readyForHotSwitch); + EXPECT_TRUE(progress.usedAirspeed); + EXPECT_FLOAT_EQ(1.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, HotSwitchWithoutAirspeedCompletesImmediatelyWhenTimerIsZero) +{ + const mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1500, + false, + 0.0f, + false, + 0.0f, + 0, + 0); + + EXPECT_TRUE(progress.readyForHotSwitch); + EXPECT_FALSE(progress.usedAirspeed); + EXPECT_FALSE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(1.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, HotSwitchCapturesAndReusesStartAirspeedForFwToMc) +{ + mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1600.0f, + false, + 0.0f, + 0, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_TRUE(progress.usedAirspeed); + EXPECT_TRUE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(1600.0f, progress.transitionStartAirspeedCmS); + EXPECT_FLOAT_EQ(0.0f, progress.progress); + + progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1300.0f, + progress.transitionStartAirspeedCaptured, + progress.transitionStartAirspeedCmS, + 0, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_FLOAT_EQ(0.50f, progress.progress); + + progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 900.0f, + progress.transitionStartAirspeedCaptured, + progress.transitionStartAirspeedCmS, + 0, + 1000); + + EXPECT_TRUE(progress.readyForHotSwitch); + EXPECT_FLOAT_EQ(1.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskCapturesOldLiftAndNewPusherButNotSharedTiltMotors) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; // shared tilt motor + currentMixer[1].throttle = 1.0f; // old lift motor + targetMixer[0].throttle = 1.0f; // shared tilt motor + targetMixer[2].throttle = 1.0f; // FW pusher appears after switch + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 3, + currentMixer, + targetMixer); + + EXPECT_EQ((1U << 1) | (1U << 2), fadeMask.motorMask); + EXPECT_EQ((1U << 2), fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskCapturesOnlyOldPusherWhenReturningToMultirotor) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; // shared tilt motor + currentMixer[2].throttle = 1.0f; // FW pusher disappears after switch + targetMixer[0].throttle = 1.0f; // shared tilt motor + targetMixer[1].throttle = 1.0f; // MC lift motor + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_MC, + false, + 3, + currentMixer, + targetMixer); + + EXPECT_EQ((1U << 2), fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskStaysEmptyWhenDynamicScalingIsDisabled) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; + targetMixer[1].throttle = 1.0f; + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + false, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 2, + currentMixer, + targetMixer); + + EXPECT_EQ(0U, fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskStaysEmptyForSharedTiltOnlyConfiguration) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; + targetMixer[0].throttle = 1.0f; + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 1, + currentMixer, + targetMixer); + + EXPECT_EQ(0U, fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, AutoServoInputMayDecreaseDuringFwToMc) +{ + EXPECT_EQ(100, mixerTransitionUpdateServoInput( + 500, + false, + true, + true, + false, + false, + 0.20f)); +} diff --git a/src/test/unit/mixer_transition_scenarios_unittest.cc b/src/test/unit/mixer_transition_scenarios_unittest.cc new file mode 100644 index 00000000000..ff993c669a7 --- /dev/null +++ b/src/test/unit/mixer_transition_scenarios_unittest.cc @@ -0,0 +1,359 @@ +#include + +extern "C" { +#include "flight/mixer_transition_logic.h" +} + +namespace { + +struct ManualTransitionScenario { + explicit ManualTransitionScenario(const bool transitionToFixedWing) + : transitionToFw(transitionToFixedWing) + { + } + + void setTransitionMode(const bool active, const bool manualControllerConfigured, const bool clearSession = false) + { + const bool risingEdge = active && !transitionModeActive; + const bool fallingEdge = !active && transitionModeActive; + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + risingEdge, + fallingEdge, + manualControllerConfigured, + clearSession); + transitionModeActive = active; + } + + bool autoControllerEnabled(const bool manualControllerConfigured) const + { + return mixerTransitionManualControllerEnabled(manualControllerConfigured, sessionMode); + } + + int16_t updateServo( + const bool transitionMixingActive, + const bool autoTransitionActive, + const bool postSwitchFadeToFwActive, + const float blendToFw) + { + servoInput = mixerTransitionUpdateServoInput( + servoInput, + sessionMode == MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + transitionMixingActive, + autoTransitionActive, + postSwitchFadeToFwActive, + transitionToFw, + blendToFw); + return servoInput; + } + + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + int16_t servoInput = 0; + bool transitionModeActive = false; + bool transitionToFw = true; +}; + +float computeBlendToFwForStep( + const mixerProfileATDirection_e direction, + float *handoffProgress, + const bool usedAirspeed, + const float rawProgress, + const float motorRampProgress) +{ + *handoffProgress = mixerTransitionResolveHandoffProgress( + true, + usedAirspeed, + *handoffProgress, + rawProgress); + + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + true, + direction, + 0.20f, + 0.30f, + 0.40f, + *handoffProgress, + motorRampProgress); + + return scales.blendToFw; +} + +} // namespace + +TEST(MixerTransitionScenarioTest, LegacyManualMcToFwSessionStaysLegacyAcrossProfileHotSwitch) +{ + ManualTransitionScenario scenario(true); + + scenario.setTransitionMode(true, false); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_LEGACY, scenario.sessionMode); + EXPECT_FALSE(scenario.autoControllerEnabled(false)); + EXPECT_EQ(500, scenario.updateServo(true, false, false, 0.18f)); + + // A hot-switch onto a profile that enables the new controller must not + // retroactively convert the already-running legacy session. + scenario.setTransitionMode(true, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_LEGACY, scenario.sessionMode); + EXPECT_FALSE(scenario.autoControllerEnabled(true)); + EXPECT_EQ(500, scenario.updateServo(true, true, false, 0.02f)); + + scenario.setTransitionMode(false, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, scenario.sessionMode); + EXPECT_EQ(0, scenario.updateServo(false, false, false, 0.0f)); +} + +TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedDropoutAndHotSwitch) +{ + ManualTransitionScenario scenario(true); + float handoffProgress = 0.0f; + + scenario.setTransitionMode(true, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); + + mixerTransitionHotSwitchProgress_t hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + true, + 320.0f, + false, + 0.0f, + 0, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(296, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeBlendToFwForStep( + MIXERAT_DIRECTION_TO_FW, + &handoffProgress, + hotSwitchProgress.usedAirspeed, + hotSwitchProgress.progress, + 0.25f))); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + true, + 650.0f, + false, + 0.0f, + 0, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(395, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeBlendToFwForStep( + MIXERAT_DIRECTION_TO_FW, + &handoffProgress, + hotSwitchProgress.usedAirspeed, + hotSwitchProgress.progress, + 0.55f))); + + // After a profile hot-switch, the active auto session must survive even if + // the target profile itself has the manual controller disabled. + scenario.setTransitionMode(true, false); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); + EXPECT_TRUE(scenario.autoControllerEnabled(false)); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + false, + 0.0f, + false, + 0.0f, + 500, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(395, scenario.updateServo( + true, + scenario.autoControllerEnabled(false), + false, + computeBlendToFwForStep( + MIXERAT_DIRECTION_TO_FW, + &handoffProgress, + hotSwitchProgress.usedAirspeed, + hotSwitchProgress.progress, + 0.75f))); + + // Even if the raw blend source drops during the same control cycle as the + // hot-switch, the MC->FW servo source must not move backwards. + EXPECT_EQ(395, scenario.updateServo( + false, + scenario.autoControllerEnabled(false), + false, + 0.20f)); + EXPECT_EQ(500, scenario.updateServo( + false, + scenario.autoControllerEnabled(false), + true, + 0.20f)); + + scenario.setTransitionMode(false, false); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, scenario.sessionMode); + EXPECT_EQ(0, scenario.updateServo(false, false, false, 0.0f)); +} + +TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturnsTowardMultirotor) +{ + ManualTransitionScenario scenario(false); + float handoffProgress = 0.0f; + + scenario.setTransitionMode(true, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); + + mixerTransitionHotSwitchProgress_t hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1600.0f, + false, + 0.0f, + 0, + 1000); + EXPECT_TRUE(hotSwitchProgress.transitionStartAirspeedCaptured); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeBlendToFwForStep( + MIXERAT_DIRECTION_TO_MC, + &handoffProgress, + hotSwitchProgress.usedAirspeed, + hotSwitchProgress.progress, + 0.0f))); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1300.0f, + hotSwitchProgress.transitionStartAirspeedCaptured, + hotSwitchProgress.transitionStartAirspeedCmS, + 0, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(350, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeBlendToFwForStep( + MIXERAT_DIRECTION_TO_MC, + &handoffProgress, + hotSwitchProgress.usedAirspeed, + hotSwitchProgress.progress, + 0.50f))); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 900.0f, + hotSwitchProgress.transitionStartAirspeedCaptured, + hotSwitchProgress.transitionStartAirspeedCmS, + 0, + 1000); + EXPECT_TRUE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(200, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeBlendToFwForStep( + MIXERAT_DIRECTION_TO_MC, + &handoffProgress, + hotSwitchProgress.usedAirspeed, + hotSwitchProgress.progress, + 1.0f))); + + scenario.setTransitionMode(false, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, scenario.sessionMode); + EXPECT_EQ(0, scenario.updateServo(false, false, false, 0.0f)); +} + +TEST(MixerTransitionScenarioTest, PusherHotSwitchFadeScenarioCapturesOnlyNonSharedMotors) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + ManualTransitionScenario scenario(true); + float handoffProgress = 0.0f; + + currentMixer[0].throttle = 1.0f; // shared tilt motor + currentMixer[1].throttle = 1.0f; // lift motor disappears after switch + targetMixer[0].throttle = 1.0f; // shared tilt motor + targetMixer[2].throttle = 1.0f; // FW pusher appears after switch + + scenario.setTransitionMode(true, true); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeBlendToFwForStep( + MIXERAT_DIRECTION_TO_FW, + &handoffProgress, + true, + 1.0f, + 1.0f))); + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 3, + currentMixer, + targetMixer); + + EXPECT_EQ((1U << 1) | (1U << 2), fadeMask.motorMask); + EXPECT_EQ((1U << 2), fadeMask.toCurrentMotorMask); + EXPECT_EQ(500, scenario.updateServo( + false, + scenario.autoControllerEnabled(true), + true, + 0.0f)); +} + +TEST(MixerTransitionScenarioTest, SharedTiltOnlyHotSwitchScenarioNeedsNoFadeAndStaysAtFwEndpoint) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + ManualTransitionScenario scenario(true); + float handoffProgress = 0.0f; + + currentMixer[0].throttle = 1.0f; + targetMixer[0].throttle = 1.0f; + + scenario.setTransitionMode(true, true); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeBlendToFwForStep( + MIXERAT_DIRECTION_TO_FW, + &handoffProgress, + true, + 1.0f, + 1.0f))); + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 1, + currentMixer, + targetMixer); + + EXPECT_EQ(0U, fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); + + // With shared tilt motors there is no separate fade mask, but the FW + // endpoint still must not fall back during the handoff cycle. + EXPECT_EQ(500, scenario.updateServo( + false, + scenario.autoControllerEnabled(true), + false, + 0.0f)); +} From 270714d8080da60407ffcbb4fae8a71dc205dcf6 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Mon, 15 Jun 2026 12:46:31 +0300 Subject: [PATCH 31/42] Fix VTOL servo transition regressions across auto and profile handoff --- src/main/flight/mixer_profile.c | 223 +++++++++++++++++- src/main/flight/mixer_profile.h | 5 + src/main/flight/mixer_transition_logic.h | 61 +++++ src/main/flight/servos.c | 7 + .../unit/mixer_transition_logic_unittest.cc | 75 ++++++ .../mixer_transition_scenarios_unittest.cc | 90 ++++--- 6 files changed, 410 insertions(+), 51 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index c2fa69eaa9a..c4c0fc7ec61 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -48,6 +48,9 @@ static bool manualTransitionReadyForEdge = true; static mixerTransitionManualSessionMode_e manualTransitionSessionMode; static bool manualFwToMcProtectionLatched; static int16_t mixerTransitionServoInput; +// Servos need their own short handoff because profile reload can change the +// final servo mix abruptly even when motor-scale debug values stay neutral. +#define MIXER_TRANSITION_SERVO_HANDOFF_FADE_MS 100U #endif // Keep PG version split because USE_AUTO_TRANSITION changes the stored mixer profile layout only on >512 KB targets. @@ -167,6 +170,176 @@ void setMixerProfileAT(void) } #ifdef USE_AUTO_TRANSITION +static void clearServoHandoffFade(void) +{ + mixerProfileAT.servoHandoffMask = 0; + mixerProfileAT.servoHandoffDurationMs = 0; + mixerProfileAT.servoHandoffStartTime = 0; + memset(mixerProfileAT.servoHandoffOutput, 0, sizeof(mixerProfileAT.servoHandoffOutput)); +} + +static bool isServoTransitionLinkedInputSource(const uint8_t inputSource) +{ + return inputSource == INPUT_MIXER_TRANSITION || + (inputSource >= INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL && + inputSource <= INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS); +} + +static uint32_t collectServoTargetMask(const servoMixer_t *rules) +{ + uint32_t mask = 0; + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + if (rules[i].rate == 0) { + break; + } + + if (rules[i].targetChannel < MAX_SUPPORTED_SERVOS) { + mask |= 1U << rules[i].targetChannel; + } + } + + return mask; +} + +static uint32_t collectTransitionLinkedServoTargetMask(const servoMixer_t *rules) +{ + uint32_t mask = 0; + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + if (rules[i].rate == 0) { + break; + } + + if (rules[i].targetChannel >= MAX_SUPPORTED_SERVOS || + !isServoTransitionLinkedInputSource(rules[i].inputSource)) { + continue; + } + + mask |= 1U << rules[i].targetChannel; + } + + return mask; +} + +static bool servoMixerRuleMatches(const servoMixer_t *lhs, const servoMixer_t *rhs) +{ + return lhs->targetChannel == rhs->targetChannel && + lhs->inputSource == rhs->inputSource && + lhs->rate == rhs->rate && + lhs->speed == rhs->speed +#ifdef USE_PROGRAMMING_FRAMEWORK + && lhs->conditionId == rhs->conditionId +#endif + ; +} + +static int collectServoRulesForTarget(const servoMixer_t *rules, const uint8_t targetChannel, const servoMixer_t *targetRules[MAX_SERVO_RULES]) +{ + int ruleCount = 0; + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + if (rules[i].rate == 0) { + break; + } + + if (rules[i].targetChannel != targetChannel) { + continue; + } + + targetRules[ruleCount++] = &rules[i]; + } + + return ruleCount; +} + +static bool servoTargetRulesDiffer(const servoMixer_t *currentRules, const servoMixer_t *targetRules, const uint8_t targetChannel) +{ + const servoMixer_t *currentTargetRules[MAX_SERVO_RULES]; + const servoMixer_t *nextTargetRules[MAX_SERVO_RULES]; + const int currentRuleCount = collectServoRulesForTarget(currentRules, targetChannel, currentTargetRules); + const int nextRuleCount = collectServoRulesForTarget(targetRules, targetChannel, nextTargetRules); + + if (currentRuleCount != nextRuleCount) { + return true; + } + + for (int i = 0; i < currentRuleCount; i++) { + if (!servoMixerRuleMatches(currentTargetRules[i], nextTargetRules[i])) { + return true; + } + } + + return false; +} + +static uint32_t collectServoProfileDifferenceMask(const servoMixer_t *currentRules, const servoMixer_t *targetRules) +{ + const uint32_t allTargetsMask = collectServoTargetMask(currentRules) | collectServoTargetMask(targetRules); + uint32_t differenceMask = 0; + + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + if ((allTargetsMask & (1U << i)) == 0) { + continue; + } + + if (servoTargetRulesDiffer(currentRules, targetRules, i)) { + differenceMask |= 1U << i; + } + } + + return differenceMask; +} + +static uint32_t collectServoHandoffMask(const int targetProfileIndex, const bool includeProfileDifferences) +{ + const servoMixer_t *currentRules = mixerServoMixersByIndex(currentMixerProfileIndex); + uint32_t handoffMask = collectTransitionLinkedServoTargetMask(currentRules); + + if (targetProfileIndex < 0 || targetProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + return handoffMask; + } + + const servoMixer_t *targetRules = mixerServoMixersByIndex(targetProfileIndex); + + handoffMask |= collectTransitionLinkedServoTargetMask(targetRules); + + if (includeProfileDifferences) { + handoffMask |= collectServoProfileDifferenceMask(currentRules, targetRules); + } + + return handoffMask; +} + +static void prepareServoHandoffFade(const uint32_t handoffMask) +{ + clearServoHandoffFade(); + + if (handoffMask == 0) { + return; + } + + mixerProfileAT.servoHandoffMask = handoffMask; + mixerProfileAT.servoHandoffDurationMs = MIXER_TRANSITION_SERVO_HANDOFF_FADE_MS; + + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + if ((handoffMask & (1U << i)) == 0) { + continue; + } + + mixerProfileAT.servoHandoffOutput[i] = servo[i]; + } +} + +static void startServoHandoffFade(void) +{ + if (mixerProfileAT.servoHandoffMask == 0 || mixerProfileAT.servoHandoffDurationMs == 0) { + return; + } + + mixerProfileAT.servoHandoffStartTime = millis(); +} + static bool requestTransitionsToFixedWing(const mixerProfileATRequest_e required_action) { return required_action == MIXERAT_REQUEST_RTH || @@ -442,6 +615,12 @@ static void updateTransitionScales(void) static void abortTransition(const bool byAirspeedTimeout) { const bool wasActive = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; + const bool hotSwitchAlreadyDone = mixerProfileAT.hotSwitchDone; + + if (wasActive && !hotSwitchAlreadyDone) { + prepareServoHandoffFade(collectServoHandoffMask(nextMixerProfileIndex, false)); + } + isMixerTransitionMixing_requested = false; mixerProfileAT.phase = MIXERAT_PHASE_IDLE; mixerProfileAT.aborted = wasActive; @@ -453,6 +632,10 @@ static void abortTransition(const bool byAirspeedTimeout) mixerProfileAT.transitionStartAirspeedCaptured = false; mixerProfileAT.transitionStartAirspeedCmS = 0.0f; resetTransitionScales(); + + if (wasActive && !hotSwitchAlreadyDone) { + startServoHandoffFade(); + } } static bool mixerATReadyForHotSwitch(const mixerProfileATRequest_e required_action) @@ -586,12 +769,14 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) isMixerTransitionMixing_requested = false; mixerProfileAT.progress = 1.0f; updateTransitionScales(); + prepareServoHandoffFade(collectServoHandoffMask(nextMixerProfileIndex, true)); preparePostSwitchFade(nextMixerProfileIndex); if (!outputProfileHotSwitch(nextMixerProfileIndex)) { abortTransition(false); return true; } mixerProfileAT.hotSwitchDone = true; + startServoHandoffFade(); if (!startPostSwitchFade()) { mixerProfileAT.phase = MIXERAT_PHASE_IDLE; mixerProfileAT.request = MIXERAT_REQUEST_NONE; @@ -950,6 +1135,18 @@ int16_t mixerATGetTransitionServoInput(void) const bool postSwitchFadeToFwActive = mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE && mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW; + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + const uint32_t transitionTimerMs = MAX(0, currentMixerConfig.switchTransitionTimer) * 100; + const float servoBlendToFw = mixerTransitionComputeServoBlendToFw( + isLegacyManualTransitionSessionActive(), + isMixerTransitionMixing, + mixerATIsActive(), + postSwitchFadeToFwActive, + mixerProfileAT.direction, + mixerProfileAT.usedAirspeed, + mixerProfileAT.progress, + elapsedMs, + transitionTimerMs); mixerTransitionServoInput = mixerTransitionUpdateServoInput( mixerTransitionServoInput, @@ -958,7 +1155,7 @@ int16_t mixerATGetTransitionServoInput(void) mixerATIsActive(), postSwitchFadeToFwActive, mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW, - mixerATGetBlendToFw()); + servoBlendToFw); return mixerTransitionServoInput; #else @@ -967,6 +1164,30 @@ int16_t mixerATGetTransitionServoInput(void) } #ifdef USE_AUTO_TRANSITION +bool mixerATGetServoHandoffOutput(uint8_t servoIndex, int16_t currentOutput, int16_t *output) +{ + if (servoIndex >= MAX_SUPPORTED_SERVOS || + mixerProfileAT.servoHandoffMask == 0 || + (mixerProfileAT.servoHandoffMask & (1U << servoIndex)) == 0) { + return false; + } + + const float progress = mixerProfileAT.servoHandoffDurationMs == 0 ? + 1.0f : + constrainf((float)(millis() - mixerProfileAT.servoHandoffStartTime) / (float)mixerProfileAT.servoHandoffDurationMs, 0.0f, 1.0f); + + *output = mixerTransitionBlendCapturedServoOutput( + mixerProfileAT.servoHandoffOutput[servoIndex], + currentOutput, + progress); + + if (progress >= 1.0f) { + clearServoHandoffFade(); + } + + return true; +} + bool mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output) { if (mixerProfileAT.phase != MIXERAT_PHASE_POST_SWITCH_FADE || diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 0b72dd5116c..92e1cedfc50 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -96,6 +96,10 @@ typedef struct mixerProfileAT_s { uint16_t postSwitchFadeDurationMs; uint16_t postSwitchFadeMotorOutput[MAX_SUPPORTED_MOTORS]; timeMs_t postSwitchFadeStartTime; + uint32_t servoHandoffMask; + uint16_t servoHandoffDurationMs; + int16_t servoHandoffOutput[MAX_SUPPORTED_SERVOS]; + timeMs_t servoHandoffStartTime; timeMs_t transitionStartTime; #else bool transitionInputMixing; @@ -119,6 +123,7 @@ int16_t mixerATGetTransitionServoInput(void); #ifdef USE_AUTO_TRANSITION bool mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output); float mixerATGetPostSwitchFadeProgress(void); +bool mixerATGetServoHandoffOutput(uint8_t servoIndex, int16_t currentOutput, int16_t *output); #endif bool isMixerProfile2ModeReportedActive(void); bool isMixerTransitionModeReportedActive(void); diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h index 1037328434a..8f7b1da455e 100644 --- a/src/main/flight/mixer_transition_logic.h +++ b/src/main/flight/mixer_transition_logic.h @@ -292,6 +292,46 @@ static inline mixerTransitionPostSwitchFadeMask_t mixerTransitionComputePostSwit } #endif +#ifdef USE_AUTO_TRANSITION +static inline float mixerTransitionComputeServoBlendToFw( + bool legacyManualTransitionActive, + bool transitionMixingActive, + bool autoTransitionActive, + bool postSwitchFadeToFwActive, + mixerProfileATDirection_e direction, + bool usedAirspeed, + float transitionProgress, + uint32_t elapsedMs, + uint32_t transitionTimerMs) +{ + if (legacyManualTransitionActive) { + return transitionMixingActive ? 1.0f : 0.0f; + } + + if (postSwitchFadeToFwActive) { + return 1.0f; + } + + if (!autoTransitionActive || direction == MIXERAT_DIRECTION_NONE) { + return 0.0f; + } + + float progress = mixerTransitionClamp(transitionProgress, 0.0f, 1.0f); + + if (!usedAirspeed) { + progress = transitionTimerMs > 0 ? + mixerTransitionClamp((float)elapsedMs / (float)transitionTimerMs, 0.0f, 1.0f) : + 1.0f; + } + + if (direction == MIXERAT_DIRECTION_TO_MC) { + return 1.0f - progress; + } + + return progress; +} +#endif + static inline int16_t mixerTransitionBlendToServoInput(float blendToFw) { if (blendToFw <= 0.0f) { @@ -337,3 +377,24 @@ static inline int16_t mixerTransitionUpdateServoInput( return desiredInput; } + +static inline int16_t mixerTransitionBlendCapturedServoOutput( + int16_t capturedOutput, + int16_t currentOutput, + float progress) +{ + const float holdScale = 1.0f - mixerTransitionClamp(progress, 0.0f, 1.0f); + const float fadedOutput = currentOutput + (capturedOutput - currentOutput) * holdScale; + const int16_t minOutput = capturedOutput < currentOutput ? capturedOutput : currentOutput; + const int16_t maxOutput = capturedOutput > currentOutput ? capturedOutput : currentOutput; + + if (fadedOutput <= minOutput) { + return minOutput; + } + + if (fadedOutput >= maxOutput) { + return maxOutput; + } + + return (int16_t)(fadedOutput + (fadedOutput >= 0.0f ? 0.5f : -0.5f)); +} diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 1680eb87c86..64f4166bc69 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -618,6 +618,13 @@ void servoMixer(float dT) * allowed the situation when smix weight sum for an output was above 100 */ servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max); + +#ifdef USE_AUTO_TRANSITION + int16_t handoffOutput = 0; + if (mixerATGetServoHandoffOutput(i, servo[i], &handoffOutput)) { + servo[i] = constrain(handoffOutput, servoParams(i)->min, servoParams(i)->max); + } +#endif } } diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc index f7f3478a921..767dfea9abf 100644 --- a/src/test/unit/mixer_transition_logic_unittest.cc +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -151,3 +151,78 @@ TEST(MixerTransitionLogicTest, AutoServoInputDoesNotMoveBackwardsDuringMcToFw) 0.0f); EXPECT_EQ(0, servoInput); } + +TEST(MixerTransitionLogicTest, AutoServoBlendUsesTransitionProgressInsteadOfNeutralAuthorityScales) +{ + const float blendAtTransitionStart = mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, + MIXERAT_DIRECTION_TO_FW, + false, + 0.0f, + 10, + 16000); + + EXPECT_LT(blendAtTransitionStart, 0.01f); + EXPECT_EQ(0, mixerTransitionUpdateServoInput( + 0, + false, + true, + true, + false, + true, + blendAtTransitionStart)); + + EXPECT_FLOAT_EQ(0.65f, mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, + MIXERAT_DIRECTION_TO_FW, + true, + 0.65f, + 0, + 0)); +} + +TEST(MixerTransitionLogicTest, AutoServoBlendCountsBackDownDuringFwToMc) +{ + EXPECT_FLOAT_EQ(0.75f, mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, + MIXERAT_DIRECTION_TO_MC, + true, + 0.25f, + 0, + 0)); + + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeServoBlendToFw( + false, + false, + true, + true, + MIXERAT_DIRECTION_TO_FW, + false, + 0.0f, + 0, + 0)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffBlendStartsFromCapturedOutputAfterHotSwitch) +{ + EXPECT_EQ(1366, mixerTransitionBlendCapturedServoOutput(1366, 980, 0.0f)); + EXPECT_EQ(1270, mixerTransitionBlendCapturedServoOutput(1366, 980, 0.25f)); + EXPECT_EQ(1173, mixerTransitionBlendCapturedServoOutput(1366, 980, 0.50f)); + EXPECT_EQ(980, mixerTransitionBlendCapturedServoOutput(1366, 980, 1.0f)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffBlendReturnsSmoothlyAfterAbort) +{ + EXPECT_EQ(1651, mixerTransitionBlendCapturedServoOutput(1651, 1224, 0.0f)); + EXPECT_EQ(1438, mixerTransitionBlendCapturedServoOutput(1651, 1224, 0.50f)); + EXPECT_EQ(1224, mixerTransitionBlendCapturedServoOutput(1651, 1224, 1.0f)); +} diff --git a/src/test/unit/mixer_transition_scenarios_unittest.cc b/src/test/unit/mixer_transition_scenarios_unittest.cc index ff993c669a7..efe74909b51 100644 --- a/src/test/unit/mixer_transition_scenarios_unittest.cc +++ b/src/test/unit/mixer_transition_scenarios_unittest.cc @@ -54,29 +54,23 @@ struct ManualTransitionScenario { bool transitionToFw = true; }; -float computeBlendToFwForStep( +float computeTransitionServoBlendForStep( const mixerProfileATDirection_e direction, - float *handoffProgress, const bool usedAirspeed, const float rawProgress, - const float motorRampProgress) + const uint32_t elapsedMs, + const uint32_t transitionTimerMs) { - *handoffProgress = mixerTransitionResolveHandoffProgress( + return mixerTransitionComputeServoBlendToFw( + false, true, - usedAirspeed, - *handoffProgress, - rawProgress); - - const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( true, + false, direction, - 0.20f, - 0.30f, - 0.40f, - *handoffProgress, - motorRampProgress); - - return scales.blendToFw; + usedAirspeed, + rawProgress, + elapsedMs, + transitionTimerMs); } } // namespace @@ -105,7 +99,6 @@ TEST(MixerTransitionScenarioTest, LegacyManualMcToFwSessionStaysLegacyAcrossProf TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedDropoutAndHotSwitch) { ManualTransitionScenario scenario(true); - float handoffProgress = 0.0f; scenario.setTransitionMode(true, true); EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); @@ -120,16 +113,16 @@ TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedD 0, 1000); EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); - EXPECT_EQ(296, scenario.updateServo( + EXPECT_EQ(160, scenario.updateServo( true, scenario.autoControllerEnabled(true), false, - computeBlendToFwForStep( + computeTransitionServoBlendForStep( MIXERAT_DIRECTION_TO_FW, - &handoffProgress, hotSwitchProgress.usedAirspeed, hotSwitchProgress.progress, - 0.25f))); + 0, + 1000))); hotSwitchProgress = mixerTransitionEvaluateHotSwitch( MIXERAT_DIRECTION_TO_FW, @@ -141,16 +134,16 @@ TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedD 0, 1000); EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); - EXPECT_EQ(395, scenario.updateServo( + EXPECT_EQ(325, scenario.updateServo( true, scenario.autoControllerEnabled(true), false, - computeBlendToFwForStep( + computeTransitionServoBlendForStep( MIXERAT_DIRECTION_TO_FW, - &handoffProgress, hotSwitchProgress.usedAirspeed, hotSwitchProgress.progress, - 0.55f))); + 0, + 1000))); // After a profile hot-switch, the active auto session must survive even if // the target profile itself has the manual controller disabled. @@ -168,20 +161,20 @@ TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedD 500, 1000); EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); - EXPECT_EQ(395, scenario.updateServo( + EXPECT_EQ(325, scenario.updateServo( true, scenario.autoControllerEnabled(false), false, - computeBlendToFwForStep( + computeTransitionServoBlendForStep( MIXERAT_DIRECTION_TO_FW, - &handoffProgress, hotSwitchProgress.usedAirspeed, hotSwitchProgress.progress, - 0.75f))); + 500, + 1000))); // Even if the raw blend source drops during the same control cycle as the // hot-switch, the MC->FW servo source must not move backwards. - EXPECT_EQ(395, scenario.updateServo( + EXPECT_EQ(325, scenario.updateServo( false, scenario.autoControllerEnabled(false), false, @@ -200,7 +193,6 @@ TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedD TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturnsTowardMultirotor) { ManualTransitionScenario scenario(false); - float handoffProgress = 0.0f; scenario.setTransitionMode(true, true); EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); @@ -219,12 +211,12 @@ TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturn true, scenario.autoControllerEnabled(true), false, - computeBlendToFwForStep( + computeTransitionServoBlendForStep( MIXERAT_DIRECTION_TO_MC, - &handoffProgress, hotSwitchProgress.usedAirspeed, hotSwitchProgress.progress, - 0.0f))); + 0, + 1000))); hotSwitchProgress = mixerTransitionEvaluateHotSwitch( MIXERAT_DIRECTION_TO_MC, @@ -236,16 +228,16 @@ TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturn 0, 1000); EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); - EXPECT_EQ(350, scenario.updateServo( + EXPECT_EQ(250, scenario.updateServo( true, scenario.autoControllerEnabled(true), false, - computeBlendToFwForStep( + computeTransitionServoBlendForStep( MIXERAT_DIRECTION_TO_MC, - &handoffProgress, hotSwitchProgress.usedAirspeed, hotSwitchProgress.progress, - 0.50f))); + 0, + 1000))); hotSwitchProgress = mixerTransitionEvaluateHotSwitch( MIXERAT_DIRECTION_TO_MC, @@ -257,16 +249,16 @@ TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturn 0, 1000); EXPECT_TRUE(hotSwitchProgress.readyForHotSwitch); - EXPECT_EQ(200, scenario.updateServo( + EXPECT_EQ(0, scenario.updateServo( true, scenario.autoControllerEnabled(true), false, - computeBlendToFwForStep( + computeTransitionServoBlendForStep( MIXERAT_DIRECTION_TO_MC, - &handoffProgress, hotSwitchProgress.usedAirspeed, hotSwitchProgress.progress, - 1.0f))); + 0, + 1000))); scenario.setTransitionMode(false, true); EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, scenario.sessionMode); @@ -278,7 +270,6 @@ TEST(MixerTransitionScenarioTest, PusherHotSwitchFadeScenarioCapturesOnlyNonShar motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; ManualTransitionScenario scenario(true); - float handoffProgress = 0.0f; currentMixer[0].throttle = 1.0f; // shared tilt motor currentMixer[1].throttle = 1.0f; // lift motor disappears after switch @@ -290,12 +281,12 @@ TEST(MixerTransitionScenarioTest, PusherHotSwitchFadeScenarioCapturesOnlyNonShar true, scenario.autoControllerEnabled(true), false, - computeBlendToFwForStep( + computeTransitionServoBlendForStep( MIXERAT_DIRECTION_TO_FW, - &handoffProgress, true, 1.0f, - 1.0f))); + 1000, + 1000))); const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( true, @@ -320,7 +311,6 @@ TEST(MixerTransitionScenarioTest, SharedTiltOnlyHotSwitchScenarioNeedsNoFadeAndS motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; ManualTransitionScenario scenario(true); - float handoffProgress = 0.0f; currentMixer[0].throttle = 1.0f; targetMixer[0].throttle = 1.0f; @@ -330,12 +320,12 @@ TEST(MixerTransitionScenarioTest, SharedTiltOnlyHotSwitchScenarioNeedsNoFadeAndS true, scenario.autoControllerEnabled(true), false, - computeBlendToFwForStep( + computeTransitionServoBlendForStep( MIXERAT_DIRECTION_TO_FW, - &handoffProgress, true, 1.0f, - 1.0f))); + 1000, + 1000))); const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( true, From 8af084da16df6088f68b8877ed650c5d64cab20a Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Mon, 15 Jun 2026 17:32:17 +0300 Subject: [PATCH 32/42] vtol: reuse scale ramp time for transition servo handoff fades --- docs/MixerProfile.md | 2 +- docs/VTOL.md | 2 +- src/main/flight/mixer_profile.c | 39 ++++-- src/main/flight/mixer_transition_logic.h | 35 ++++-- .../unit/mixer_transition_logic_unittest.cc | 68 +++++++++-- .../mixer_transition_scenarios_unittest.cc | 113 +++++++++++++----- 6 files changed, 192 insertions(+), 67 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index ced97b344fb..23a93e8ebb3 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -180,7 +180,7 @@ This preview uses the target fixed-wing PID bank, rates, angle limits, heading-h During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. At the same time, target MC motor rules can fade in before the switch and use a target MC PID preview, so lift motors are not driven by the active FW/PIFF controller. These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. -`INPUT_MIXER_TRANSITION` remains available for transition-progress servo movement such as tilt or helper servos. +`INPUT_MIXER_TRANSITION` remains available for tilt/helper servo movement. With `mixer_vtol_transition_dynamic_mixer = ON`, it follows `mixer_vtol_transition_scale_ramp_time_ms`. With it OFF, it keeps the older fixed transition behavior while the auto controller only decides the profile switch timing. If a profile hot-switch or direct profile switch still needs a servo handoff fade, that fade starts its own full `mixer_vtol_transition_scale_ramp_time_ms` window instead of using a separate fixed servo delay. Forward motor setup for smooth auto-transition: diff --git a/docs/VTOL.md b/docs/VTOL.md index 89b106969f1..edb4655a2af 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -398,7 +398,7 @@ This preview uses the target fixed-wing PID bank, rates, angle limits, heading-h During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. At the same time, target MC motor rules can fade in before the switch and use a target MC PID preview, so lift motors are not driven by the active FW/PIFF controller. These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. -`INPUT_MIXER_TRANSITION` remains available for transition-progress servo movement such as tilt or helper servos. +`INPUT_MIXER_TRANSITION` remains available for tilt/helper servo movement. With `mixer_vtol_transition_dynamic_mixer = ON`, it follows `mixer_vtol_transition_scale_ramp_time_ms`. With it OFF, it keeps the older fixed transition behavior while the auto controller only decides the profile switch timing. If a profile hot-switch or direct profile switch still needs a servo handoff fade, that fade starts its own full `mixer_vtol_transition_scale_ramp_time_ms` window instead of using a separate fixed servo delay. Forward motor setup for smooth auto-transition: diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index c4c0fc7ec61..38fea4b1eda 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -48,9 +48,6 @@ static bool manualTransitionReadyForEdge = true; static mixerTransitionManualSessionMode_e manualTransitionSessionMode; static bool manualFwToMcProtectionLatched; static int16_t mixerTransitionServoInput; -// Servos need their own short handoff because profile reload can change the -// final servo mix abruptly even when motor-scale debug values stay neutral. -#define MIXER_TRANSITION_SERVO_HANDOFF_FADE_MS 100U #endif // Keep PG version split because USE_AUTO_TRANSITION changes the stored mixer profile layout only on >512 KB targets. @@ -291,6 +288,8 @@ static uint32_t collectServoProfileDifferenceMask(const servoMixer_t *currentRul return differenceMask; } +static uint16_t getServoHandoffDurationMs(void); + static uint32_t collectServoHandoffMask(const int targetProfileIndex, const bool includeProfileDifferences) { const servoMixer_t *currentRules = mixerServoMixersByIndex(currentMixerProfileIndex); @@ -315,12 +314,14 @@ static void prepareServoHandoffFade(const uint32_t handoffMask) { clearServoHandoffFade(); - if (handoffMask == 0) { + const uint16_t handoffDurationMs = getServoHandoffDurationMs(); + + if (handoffMask == 0 || handoffDurationMs == 0) { return; } mixerProfileAT.servoHandoffMask = handoffMask; - mixerProfileAT.servoHandoffDurationMs = MIXER_TRANSITION_SERVO_HANDOFF_FADE_MS; + mixerProfileAT.servoHandoffDurationMs = handoffDurationMs; for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { if ((handoffMask & (1U << i)) == 0) { @@ -410,6 +411,16 @@ static float getMotorRampProgress(void) return mixerProfileAT.motorRampProgress; } +static uint16_t getServoHandoffDurationMs(void) +{ + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + + return mixerTransitionComputeServoHandoffDurationMs( + currentMixerConfig.vtolTransitionDynamicMixer, + currentMixerConfig.vtolTransitionScaleRampTimeMs, + elapsedMs); +} + static float getHandoffScalingProgress(void) { mixerProfileAT.handoffScalingProgress = mixerTransitionResolveHandoffProgress( @@ -914,7 +925,15 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) { if (mixerProfileModePresent && !transitionControllerOwnsProfileSwitch && !fwToMcProtectionOwnsProfileSwitch) { - outputProfileHotSwitch(requestedProfileIndex); + if (requestedProfileIndex != currentMixerProfileIndex) { + prepareServoHandoffFade(collectServoHandoffMask(requestedProfileIndex, true)); + + if (outputProfileHotSwitch(requestedProfileIndex)) { + startServoHandoffFade(); + } else { + clearServoHandoffFade(); + } + } } } @@ -1136,17 +1155,15 @@ int16_t mixerATGetTransitionServoInput(void) mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE && mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW; const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; - const uint32_t transitionTimerMs = MAX(0, currentMixerConfig.switchTransitionTimer) * 100; const float servoBlendToFw = mixerTransitionComputeServoBlendToFw( isLegacyManualTransitionSessionActive(), isMixerTransitionMixing, mixerATIsActive(), postSwitchFadeToFwActive, + currentMixerConfig.vtolTransitionDynamicMixer, mixerProfileAT.direction, - mixerProfileAT.usedAirspeed, - mixerProfileAT.progress, - elapsedMs, - transitionTimerMs); + currentMixerConfig.vtolTransitionScaleRampTimeMs, + elapsedMs); mixerTransitionServoInput = mixerTransitionUpdateServoInput( mixerTransitionServoInput, diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h index 8f7b1da455e..21de8d8366b 100644 --- a/src/main/flight/mixer_transition_logic.h +++ b/src/main/flight/mixer_transition_logic.h @@ -123,6 +123,21 @@ static inline float mixerTransitionComputeMotorRampProgress( return mixerTransitionClamp((float)elapsedMs / (float)scaleRampTimeMs, 0.0f, 1.0f); } +static inline uint16_t mixerTransitionComputeServoHandoffDurationMs( + bool dynamicMixerEnabled, + uint16_t scaleRampTimeMs, + uint32_t elapsedMs) +{ + (void)dynamicMixerEnabled; + (void)elapsedMs; + + if (scaleRampTimeMs == 0) { + return 0; + } + + return scaleRampTimeMs; +} + static inline float mixerTransitionResolveHandoffProgress( bool dynamicMixerEnabled, bool usedAirspeed, @@ -298,11 +313,10 @@ static inline float mixerTransitionComputeServoBlendToFw( bool transitionMixingActive, bool autoTransitionActive, bool postSwitchFadeToFwActive, + bool dynamicMixerEnabled, mixerProfileATDirection_e direction, - bool usedAirspeed, - float transitionProgress, - uint32_t elapsedMs, - uint32_t transitionTimerMs) + uint16_t scaleRampTimeMs, + uint32_t elapsedMs) { if (legacyManualTransitionActive) { return transitionMixingActive ? 1.0f : 0.0f; @@ -316,14 +330,15 @@ static inline float mixerTransitionComputeServoBlendToFw( return 0.0f; } - float progress = mixerTransitionClamp(transitionProgress, 0.0f, 1.0f); - - if (!usedAirspeed) { - progress = transitionTimerMs > 0 ? - mixerTransitionClamp((float)elapsedMs / (float)transitionTimerMs, 0.0f, 1.0f) : - 1.0f; + if (!dynamicMixerEnabled) { + return transitionMixingActive ? 1.0f : 0.0f; } + const float progress = mixerTransitionComputeMotorRampProgress( + dynamicMixerEnabled, + scaleRampTimeMs, + elapsedMs); + if (direction == MIXERAT_DIRECTION_TO_MC) { return 1.0f - progress; } diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc index 767dfea9abf..c2e77afa929 100644 --- a/src/test/unit/mixer_transition_logic_unittest.cc +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -152,18 +152,17 @@ TEST(MixerTransitionLogicTest, AutoServoInputDoesNotMoveBackwardsDuringMcToFw) EXPECT_EQ(0, servoInput); } -TEST(MixerTransitionLogicTest, AutoServoBlendUsesTransitionProgressInsteadOfNeutralAuthorityScales) +TEST(MixerTransitionLogicTest, AutoServoBlendUsesScaleRampTimerInsteadOfAirspeedProgress) { const float blendAtTransitionStart = mixerTransitionComputeServoBlendToFw( false, true, true, false, + true, MIXERAT_DIRECTION_TO_FW, - false, - 0.0f, - 10, - 16000); + 16000, + 10); EXPECT_LT(blendAtTransitionStart, 0.01f); EXPECT_EQ(0, mixerTransitionUpdateServoInput( @@ -180,11 +179,10 @@ TEST(MixerTransitionLogicTest, AutoServoBlendUsesTransitionProgressInsteadOfNeut true, true, false, - MIXERAT_DIRECTION_TO_FW, true, - 0.65f, - 0, - 0)); + MIXERAT_DIRECTION_TO_FW, + 1000, + 650)); } TEST(MixerTransitionLogicTest, AutoServoBlendCountsBackDownDuringFwToMc) @@ -194,22 +192,66 @@ TEST(MixerTransitionLogicTest, AutoServoBlendCountsBackDownDuringFwToMc) true, true, false, + true, MIXERAT_DIRECTION_TO_MC, + 1000, + 250)); + + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeServoBlendToFw( + false, + false, true, - 0.25f, + true, + true, + MIXERAT_DIRECTION_TO_FW, 0, 0)); +} +TEST(MixerTransitionLogicTest, AutoServoBlendStaysLegacyStaticWhenDynamicMixerIsDisabled) +{ EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeServoBlendToFw( - false, false, true, true, + false, + false, MIXERAT_DIRECTION_TO_FW, + 1000, + 0)); + + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, false, - 0.0f, + MIXERAT_DIRECTION_TO_FW, + 1000, + 100)); + + EXPECT_EQ(500, mixerTransitionUpdateServoInput( 0, - 0)); + false, + true, + true, + false, + true, + 1.0f)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffUsesFullScaleRampTimeAfterHotSwitchWhenDynamicMixerIsEnabled) +{ + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(true, 1000, 0)); + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(true, 1000, 250)); + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(true, 1000, 1200)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffUsesConfiguredScaleRampWhenDynamicMixerIsDisabled) +{ + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(false, 1000, 0)); + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(false, 1000, 750)); + EXPECT_EQ(0, mixerTransitionComputeServoHandoffDurationMs(false, 0, 750)); } TEST(MixerTransitionLogicTest, ServoHandoffBlendStartsFromCapturedOutputAfterHotSwitch) diff --git a/src/test/unit/mixer_transition_scenarios_unittest.cc b/src/test/unit/mixer_transition_scenarios_unittest.cc index efe74909b51..95004bca159 100644 --- a/src/test/unit/mixer_transition_scenarios_unittest.cc +++ b/src/test/unit/mixer_transition_scenarios_unittest.cc @@ -55,22 +55,23 @@ struct ManualTransitionScenario { }; float computeTransitionServoBlendForStep( + const bool dynamicMixerEnabled, const mixerProfileATDirection_e direction, - const bool usedAirspeed, - const float rawProgress, + const uint16_t scaleRampTimeMs, const uint32_t elapsedMs, const uint32_t transitionTimerMs) { + (void)transitionTimerMs; + return mixerTransitionComputeServoBlendToFw( false, true, true, false, + dynamicMixerEnabled, direction, - usedAirspeed, - rawProgress, - elapsedMs, - transitionTimerMs); + scaleRampTimeMs, + elapsedMs); } } // namespace @@ -113,15 +114,15 @@ TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedD 0, 1000); EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); - EXPECT_EQ(160, scenario.updateServo( + EXPECT_EQ(125, scenario.updateServo( true, scenario.autoControllerEnabled(true), false, computeTransitionServoBlendForStep( + true, MIXERAT_DIRECTION_TO_FW, - hotSwitchProgress.usedAirspeed, - hotSwitchProgress.progress, - 0, + 1000, + 250, 1000))); hotSwitchProgress = mixerTransitionEvaluateHotSwitch( @@ -134,15 +135,15 @@ TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedD 0, 1000); EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); - EXPECT_EQ(325, scenario.updateServo( + EXPECT_EQ(275, scenario.updateServo( true, scenario.autoControllerEnabled(true), false, computeTransitionServoBlendForStep( + true, MIXERAT_DIRECTION_TO_FW, - hotSwitchProgress.usedAirspeed, - hotSwitchProgress.progress, - 0, + 1000, + 550, 1000))); // After a profile hot-switch, the active auto session must survive even if @@ -161,20 +162,20 @@ TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedD 500, 1000); EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); - EXPECT_EQ(325, scenario.updateServo( + EXPECT_EQ(275, scenario.updateServo( true, scenario.autoControllerEnabled(false), false, computeTransitionServoBlendForStep( + true, MIXERAT_DIRECTION_TO_FW, - hotSwitchProgress.usedAirspeed, - hotSwitchProgress.progress, + 1000, 500, 1000))); // Even if the raw blend source drops during the same control cycle as the // hot-switch, the MC->FW servo source must not move backwards. - EXPECT_EQ(325, scenario.updateServo( + EXPECT_EQ(275, scenario.updateServo( false, scenario.autoControllerEnabled(false), false, @@ -190,6 +191,56 @@ TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedD EXPECT_EQ(0, scenario.updateServo(false, false, false, 0.0f)); } +TEST(MixerTransitionScenarioTest, AutoMcToFwSessionKeepsLegacyServoEndpointWhenDynamicMixerIsDisabled) +{ + ManualTransitionScenario scenario(true); + + scenario.setTransitionMode(true, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); + + mixerTransitionHotSwitchProgress_t hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + true, + 320.0f, + false, + 0.0f, + 0, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + false, + MIXERAT_DIRECTION_TO_FW, + 1000, + 0, + 1000))); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + false, + 0.0f, + false, + 0.0f, + 500, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + false, + MIXERAT_DIRECTION_TO_FW, + 1000, + 500, + 1000))); +} + TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturnsTowardMultirotor) { ManualTransitionScenario scenario(false); @@ -212,9 +263,9 @@ TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturn scenario.autoControllerEnabled(true), false, computeTransitionServoBlendForStep( + true, MIXERAT_DIRECTION_TO_MC, - hotSwitchProgress.usedAirspeed, - hotSwitchProgress.progress, + 1000, 0, 1000))); @@ -228,15 +279,15 @@ TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturn 0, 1000); EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); - EXPECT_EQ(250, scenario.updateServo( + EXPECT_EQ(125, scenario.updateServo( true, scenario.autoControllerEnabled(true), false, computeTransitionServoBlendForStep( + true, MIXERAT_DIRECTION_TO_MC, - hotSwitchProgress.usedAirspeed, - hotSwitchProgress.progress, - 0, + 1000, + 750, 1000))); hotSwitchProgress = mixerTransitionEvaluateHotSwitch( @@ -254,10 +305,10 @@ TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturn scenario.autoControllerEnabled(true), false, computeTransitionServoBlendForStep( + true, MIXERAT_DIRECTION_TO_MC, - hotSwitchProgress.usedAirspeed, - hotSwitchProgress.progress, - 0, + 1000, + 1000, 1000))); scenario.setTransitionMode(false, true); @@ -282,9 +333,9 @@ TEST(MixerTransitionScenarioTest, PusherHotSwitchFadeScenarioCapturesOnlyNonShar scenario.autoControllerEnabled(true), false, computeTransitionServoBlendForStep( - MIXERAT_DIRECTION_TO_FW, true, - 1.0f, + MIXERAT_DIRECTION_TO_FW, + 1000, 1000, 1000))); @@ -321,9 +372,9 @@ TEST(MixerTransitionScenarioTest, SharedTiltOnlyHotSwitchScenarioNeedsNoFadeAndS scenario.autoControllerEnabled(true), false, computeTransitionServoBlendForStep( - MIXERAT_DIRECTION_TO_FW, true, - 1.0f, + MIXERAT_DIRECTION_TO_FW, + 1000, 1000, 1000))); From d311c7df216898099cac46715e42b01ba5d4cf40 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 16 Jun 2026 16:47:33 +0300 Subject: [PATCH 33/42] Improve VTOL MC protection and landing safety Add conservative VTOL multicopter protection logic that is disabled by default and only applies to VTOL aircraft in MC mode. The protection keeps fixed-wing mode and normal multicopters unchanged. Add VTOL MC protection helpers, configuration, and debug visibility for: - detecting when VTOL MC protection is active - applying protected altitude throttle bounds before altitude PID anti-windup - preserving hover throttle inside the protected range - reporting when reserve bounds had to be shrunk - tracking capture, settle, landing-settle, bailout, and yaw-shaping state Add altitude authority protection for VTOL MC navigation: - reserve low-side and high-side throttle authority for attitude control - feed the protected throttle range into the altitude PID limits - freeze/relax altitude integrator during protection/bailout conditions - avoid allowing altitude hold to consume the full throttle range Add VTOL MC capture and settle behavior: - damp/settle after entering NAV instead of immediately latching an aggressive position target - avoid rubber-banding to the initial release point - gate normal NAV behavior on stable speed/attitude conditions - reuse existing navigation limits where practical to avoid extra settings Add conservative ANGLE/HORIZON shaping for VTOL MC protection: - only active with NAV_AND_STABILIZED protection mode - only when armed, in VTOL MC mode, not fixed-wing, with trusted velocity - scales yaw, pitch and roll continuously with horizontal speed Add VTOL MC landing settle gates: - require stable position/velocity/attitude before starting landing descent - cap large waypoint radii through a named helper instead of using raw magic constants - use waypoint landing position for mission LAND and home/final land target for RTH landing Fix mission VTOL transition handling: - add the missing FSM transition from WAYPOINT_PRE_ACTION to MIXERAT_INITIALIZE - wait at the transition waypoint until XY and altitude requirements are met - use nav_wp_enforce_altitude as the mission transition altitude tolerance when configured, otherwise fall back to a named conservative tolerance - avoid rejecting a mission transition immediately just because altitude has not been reached yet Harden multicopter landing detection: - prevent autonomous landing detection while vertical speed is still significant - keep the vertical-speed hard gate independent from nav_land_detect_sensitivity - require final slow-descent demand before autonomous NAV landing detection - require throttle to be genuinely low relative to hover and idle throttle - avoid relying on barometric absolute altitude as proof of landing - keep emergency/non-autonomous landing detection on its separate path Add focused unit coverage for VTOL MC protection logic: - default OFF and non-VTOL no-op behavior - throttle reserve bounds and reserve shrink cases - settle timer pass/reset behavior - landing radius cap behavior with large nav_wp_radius - yaw shaping continuity and no-op cases --- src/main/CMakeLists.txt | 3 + src/main/build/debug.h | 1 + src/main/fc/cli.c | 3 +- src/main/fc/config.c | 4 +- src/main/fc/config.h | 8 + src/main/fc/fc_core.c | 9 +- src/main/fc/settings.yaml | 17 +- src/main/navigation/navigation.c | 62 ++- src/main/navigation/navigation.h | 1 - src/main/navigation/navigation_multicopter.c | 44 +- src/main/navigation/navigation_private.h | 5 + .../navigation_vtol_mc_protection.c | 482 ++++++++++++++++++ .../navigation_vtol_mc_protection.h | 89 ++++ .../navigation_vtol_mc_protection_logic.h | 261 ++++++++++ src/test/unit/CMakeLists.txt | 1 + .../unit/vtol_mc_protection_logic_unittest.cc | 214 ++++++++ 16 files changed, 1185 insertions(+), 19 deletions(-) create mode 100644 src/main/navigation/navigation_vtol_mc_protection.c create mode 100644 src/main/navigation/navigation_vtol_mc_protection.h create mode 100644 src/main/navigation/navigation_vtol_mc_protection_logic.h create mode 100644 src/test/unit/vtol_mc_protection_logic_unittest.cc diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index e060e558f89..21a0fd81798 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -599,6 +599,9 @@ main_sources(COMMON_SRC navigation/navigation_pos_estimator_flow.c navigation/navigation_private.h navigation/navigation_rover_boat.c + navigation/navigation_vtol_mc_protection.c + navigation/navigation_vtol_mc_protection.h + navigation/navigation_vtol_mc_protection_logic.h navigation/navigation_geozone.c navigation/sqrt_controller.c navigation/sqrt_controller.h diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 8e585d299ab..fe1f127b9f3 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -81,6 +81,7 @@ typedef enum { DEBUG_SBUS2, DEBUG_OSD_REFRESH, DEBUG_VTOL_TRANSITION, + DEBUG_VTOL_MC_PROTECT, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 42bd968a24a..a7852c0758c 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -228,7 +228,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "LULU", "SBUS2", "OSD_REFRESH", - "VTOL_TRANSITION" + "VTOL_TRANSITION", + "VTOL_MC_PROTECT" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8cdca161e34..304766b362c 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -105,7 +105,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, // Keep PG version split because USE_AUTO_TRANSITION changes the stored layout only on >512 KB targets. #ifdef USE_AUTO_TRANSITION -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 10); #else PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 7); #endif @@ -129,6 +129,8 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .vtolTransitionLiftMinPercent = SETTING_VTOL_TRANSITION_LIFT_MIN_PERCENT_DEFAULT, .vtolTransitionMcAuthorityMinPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_MIN_PERCENT_DEFAULT, .vtolTransitionFwAuthorityMinPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_MIN_PERCENT_DEFAULT, + .vtolMcProtectionMode = SETTING_VTOL_MC_PROTECTION_MODE_DEFAULT, + .vtolMcThrReservePercent = SETTING_VTOL_MC_THR_RESERVE_PERCENT_DEFAULT, #endif .craftName = SETTING_NAME_DEFAULT, .pilotName = SETTING_NAME_DEFAULT diff --git a/src/main/fc/config.h b/src/main/fc/config.h index b22e292b032..746a9f6fd48 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -31,6 +31,12 @@ #define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz +typedef enum { + VTOL_MC_PROTECTION_OFF = 0, + VTOL_MC_PROTECTION_NAV, + VTOL_MC_PROTECTION_NAV_AND_STABILIZED, +} vtolMcProtectionMode_e; + typedef enum { FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, @@ -85,6 +91,8 @@ typedef struct systemConfig_s { uint8_t vtolTransitionLiftMinPercent; uint8_t vtolTransitionMcAuthorityMinPercent; uint8_t vtolTransitionFwAuthorityMinPercent; + uint8_t vtolMcProtectionMode; + uint8_t vtolMcThrReservePercent; #endif char craftName[MAX_NAME_LENGTH + 1]; char pilotName[MAX_NAME_LENGTH + 1]; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3223aca497e..91961bb7f0b 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -74,6 +74,7 @@ #include "msp/msp_serial.h" #include "navigation/navigation.h" +#include "navigation/navigation_vtol_mc_protection.h" #include "rx/rx.h" #include "rx/msp.h" @@ -418,14 +419,17 @@ static void processPilotAndFailSafeActions(float dT) } else { DEBUG_SET(DEBUG_RATE_DYNAMICS, 0, rcCommand[ROLL]); rcCommand[ROLL] = applyRateDynamics(rcCommand[ROLL], ROLL, dT); - DEBUG_SET(DEBUG_RATE_DYNAMICS, 1, rcCommand[ROLL]); DEBUG_SET(DEBUG_RATE_DYNAMICS, 2, rcCommand[PITCH]); rcCommand[PITCH] = applyRateDynamics(rcCommand[PITCH], PITCH, dT); - DEBUG_SET(DEBUG_RATE_DYNAMICS, 3, rcCommand[PITCH]); DEBUG_SET(DEBUG_RATE_DYNAMICS, 4, rcCommand[YAW]); rcCommand[YAW] = applyRateDynamics(rcCommand[YAW], YAW, dT); + + navigationVtolMcProtectionApplyStabilizedCommandShaping(&rcCommand[ROLL], &rcCommand[PITCH], &rcCommand[YAW]); + + DEBUG_SET(DEBUG_RATE_DYNAMICS, 1, rcCommand[ROLL]); + DEBUG_SET(DEBUG_RATE_DYNAMICS, 3, rcCommand[PITCH]); DEBUG_SET(DEBUG_RATE_DYNAMICS, 5, rcCommand[YAW]); } @@ -464,6 +468,7 @@ void disarm(disarmReason_t disarmReason) #endif statsOnDisarm(); logicConditionReset(); + navigationVtolMcProtectionResetTransientStates(); #ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fbe2d5207d4..7151e693c28 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,9 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", - "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "VTOL_TRANSITION"] + "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "VTOL_TRANSITION", "VTOL_MC_PROTECT"] + - name: vtol_mc_protection_mode + values: ["OFF", "NAV", "NAV_AND_STABILIZED"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e @@ -4073,6 +4075,19 @@ groups: field: vtolTransitionFwAuthorityMinPercent min: 0 max: 100 + - name: vtol_mc_protection_mode + description: "Enables extra protection for VTOL aircraft while the active mixer profile is multicopter-like and another mixer profile is fixed-wing. OFF keeps legacy behavior. NAV protects altitude/NAV throttle authority, NAV capture, landing settle, and conservative bailout. NAV_AND_STABILIZED also applies speed-based roll/pitch/yaw command shaping in ANGLE/HORIZON. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: "OFF" + field: vtolMcProtectionMode + table: vtol_mc_protection_mode + - name: vtol_mc_thr_reserve_percent + description: "Throttle range percentage reserved on both low and high side for VTOL MC attitude authority while navigation/altitude control owns throttle. The reserve is applied before altitude PID anti-windup bounds and is automatically shrunk if needed to keep hover throttle inside the usable range. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 15 + field: vtolMcThrReservePercent + min: 0 + max: 40 - name: name description: "Craft name" default_value: "" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 5a064663bed..ceb8a625059 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -56,6 +56,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "navigation/navigation_vtol_mc_protection.h" #include "navigation/rth_trackback.h" #include "rx/rx.h" @@ -92,6 +93,7 @@ #define NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS 6000 #define NAV_MIXERAT_RETRY_MAX_TOTAL_MS 45000 #define NAV_MIXERAT_MISSION_TRANSITION_TRACK_DISTANCE_CM 4000 +#define NAV_MIXERAT_MISSION_TRANSITION_ALTITUDE_FALLBACK_TOLERANCE_CM 100 #endif /*----------------------------------------------------------- @@ -217,7 +219,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT, // Disable boost at 1m/s .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle #endif - .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100 .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100 .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT, @@ -326,6 +327,7 @@ typedef enum { typedef enum { NAV_MISSION_VTOL_TRANSITION_NONE = 0, NAV_MISSION_VTOL_TRANSITION_CONTINUE, + NAV_MISSION_VTOL_TRANSITION_WAIT, NAV_MISSION_VTOL_TRANSITION_START, NAV_MISSION_VTOL_TRANSITION_REJECT, } navMissionVtolTransitionDisposition_e; @@ -360,6 +362,7 @@ 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); +bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing); bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); @@ -863,6 +866,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } @@ -1891,6 +1895,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LAN // If landing is not temporarily paused (FS only), position ok, OR within valid timeout - continue // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) { + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); + + if (navigationRTHAllowsLanding() && !navigationVtolMcProtectionLandingSettleReady(tmpHomePos)) { + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + return NAV_FSM_EVENT_NONE; + } + resetLandingDetector(); // force reset landing detector just in case updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land @@ -1939,6 +1951,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } + const fpVector3_t *landingSettlePos = FLIGHT_MODE(NAV_WP_MODE) ? &posControl.activeWaypoint.pos : rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND); + if (!navigationVtolMcProtectionLandingSettleReady(landingSettlePos)) { + if (FLIGHT_MODE(NAV_WP_MODE)) { + setDesiredPosition(landingSettlePos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + } else { + setDesiredPosition(landingSettlePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + } + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + return NAV_FSM_EVENT_NONE; + } + #ifdef USE_FW_AUTOLAND if (STATE(AIRPLANE)) { int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8, approachSettingIdx = -1; @@ -2098,6 +2121,33 @@ static bool isMissionTransitionToMultirotorType(const flyingPlatformType_e platf return isMultirotorTypePlatform(platformType); } +static uint16_t missionVTOLTransitionAltitudeToleranceCm(void) +{ + return navConfig()->general.waypoint_enforce_altitude > 0 ? + navConfig()->general.waypoint_enforce_altitude : + NAV_MIXERAT_MISSION_TRANSITION_ALTITUDE_FALLBACK_TOLERANCE_CM; +} + +static bool missionVTOLTransitionPointReady(const bool transitionToFixedWing, const uint16_t transitionMinAltitude) +{ + if (!isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) { + return false; + } + + const float currentAltitude = navGetCurrentActualPositionAndVelocity()->pos.z; + + if (transitionMinAltitude > 0 && currentAltitude < transitionMinAltitude) { + return false; + } + + if (transitionToFixedWing && + currentAltitude < (posControl.activeWaypoint.pos.z - missionVTOLTransitionAltitudeToleranceCm())) { + return false; + } + + return true; +} + #ifdef USE_PITOT static bool hasTrustedPitotAirspeed(float *airspeedCmS) { @@ -2350,6 +2400,8 @@ static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const return NAV_MISSION_VTOL_TRANSITION_CONTINUE; } + const uint16_t transitionMinAltitude = navConfig()->general.vtol_mission_transition_min_altitude; + if (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating() || @@ -2361,9 +2413,8 @@ static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const 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; + if (!missionVTOLTransitionPointReady(transitionToFixedWing, transitionMinAltitude)) { + return NAV_MISSION_VTOL_TRANSITION_WAIT; } const flyingPlatformType_e targetPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; @@ -2432,6 +2483,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav #ifdef USE_AUTO_TRANSITION clearMissionVTOLTransitionState(); const navMissionVtolTransitionDisposition_e transitionAction = prepareMissionVTOLTransition(activeWaypoint); + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_WAIT) { + return NAV_FSM_EVENT_NONE; + } if (transitionAction == NAV_MISSION_VTOL_TRANSITION_START) { return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 42f7c7737f0..36152a7a117 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -490,7 +490,6 @@ typedef struct navConfig_s { uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase #endif - uint8_t posDecelerationTime; // Brake time parameter uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC) bool slowDownForTurning; // Slow down during WP missions when changing heading on next waypoint diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index d3099eb2465..61c6d122744 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -51,6 +51,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" #include "navigation/sqrt_controller.h" +#include "navigation/navigation_vtol_mc_protection.h" #include "sensors/battery.h" @@ -77,6 +78,8 @@ float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros) // Position to velocity controller for Z axis static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) { + navigationVtolMcProtectionApplySoftAltitudeCapture(navGetCurrentStateFlags()); + float targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); posControl.pids.pos[Z].output_constrained = targetVel; // only used for Blackbox and OSD info @@ -108,15 +111,23 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) { // Calculate min and max throttle boundaries (to compensate for integral windup) - const int16_t thrCorrectionMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle; - const int16_t thrCorrectionMax = getMaxThrottle() - currentBatteryProfile->nav.mc.hover_throttle; - - float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, 0); + const int16_t idleThrottle = getThrottleIdleValue(); + const int16_t hoverThrottle = currentBatteryProfile->nav.mc.hover_throttle; + const int16_t maxThrottle = getMaxThrottle(); + const vtolMcProtectionThrottleBounds_t vtolMcThrottleBounds = navigationVtolMcProtectionGetThrottleBounds(idleThrottle, hoverThrottle, maxThrottle); + const int16_t thrCorrectionMin = vtolMcThrottleBounds.min - hoverThrottle; + const int16_t thrCorrectionMax = vtolMcThrottleBounds.max - hoverThrottle; + const pidControllerFlags_e pidFlags = navigationVtolMcProtectionShouldFreezeAltitudeIntegrator() ? PID_FREEZE_INTEGRATOR : 0; + + float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, pidFlags); int16_t rcThrottleCorrection = pt1FilterApply3(&altholdThrottleFilterState, velocity_controller, US2S(deltaMicros)); rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax); - posControl.rcAdjustment[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, false); + int16_t protectedThrottle = hoverThrottle + rcThrottleCorrection; + protectedThrottle = navigationVtolMcProtectionApplyBailoutThrottle(protectedThrottle, &vtolMcThrottleBounds, hoverThrottle); + posControl.rcAdjustment[THROTTLE] = setDesiredThrottle(protectedThrottle, false); + navigationVtolMcProtectionPublishThrottleDebug(&vtolMcThrottleBounds, posControl.rcAdjustment[THROTTLE]); } bool adjustMulticopterAltitudeFromRCInput(void) @@ -731,6 +742,10 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) // If we have new position data - update velocity and acceleration controllers if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { + if (navigationVtolMcProtectionApplyCapture(navGetCurrentStateFlags())) { + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); + } + // Get max speed for current NAV mode float maxSpeed = getActiveSpeed(); updatePositionVelocityController_MC(maxSpeed); @@ -869,9 +884,14 @@ bool isMulticopterLandingDetected(void) const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f; + const float absVerticalSpeed = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z); + // check vertical and horizontal velocities are low (cm/s) - bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (MC_LAND_CHECK_VEL_Z_MOVING * sensitivity) && + bool velCondition = absVerticalSpeed < (MC_LAND_CHECK_VEL_Z_MOVING * sensitivity) && posControl.actualState.velXY < (MC_LAND_CHECK_VEL_XY_MOVING * sensitivity); + // Sensitivity may relax the generic velocity check, but auto-disarm still needs + // the aircraft to be nearly stopped vertically when a Z estimate is available. + const bool verticalSpeedSettled = posControl.flags.estAltStatus == EST_NONE || absVerticalSpeed < MC_LAND_DETECT_MAX_VEL_Z; // check gyro rates are low (degs/s) bool gyroCondition = averageAbsGyroRates() < (4.0f * sensitivity); DEBUG_SET(DEBUG_LANDING, 2, velCondition); @@ -904,14 +924,20 @@ bool isMulticopterLandingDetected(void) landingThrSum += rcCommandAdjustedThrottle; isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE); - possibleLandingDetected = isAtMinimalThrust && velCondition; + const float finalDescentDemandLimit = navConfig()->general.land_minalt_vspd + MC_LAND_DETECT_DESCENT_DEMAND_MARGIN; + const bool descentDemandSettled = posControl.desiredState.vel.z > -finalDescentDemandLimit; + const int16_t landingThrottleLimit = (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()) / 2; + const bool throttleLowEnough = rcCommandAdjustedThrottle < landingThrottleLimit; + + possibleLandingDetected = isAtMinimalThrust && throttleLowEnough && + velCondition && verticalSpeedSettled && descentDemandSettled; DEBUG_SET(DEBUG_LANDING, 6, rcCommandAdjustedThrottle); DEBUG_SET(DEBUG_LANDING, 7, landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE); } else { // non autonomous and emergency landing DEBUG_SET(DEBUG_LANDING, 4, 2); if (landingDetectorStartedAt) { - possibleLandingDetected = velCondition && gyroCondition; + possibleLandingDetected = velCondition && verticalSpeedSettled && gyroCondition; } else { landingDetectorStartedAt = currentTimeMs; return false; @@ -1037,4 +1063,4 @@ void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlag if (navStateFlags & NAV_CTL_YAW) applyMulticopterHeadingController(); } -} \ No newline at end of file +} diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index c4296213f43..56292b34813 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -41,6 +41,11 @@ #define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s #define MC_LAND_CHECK_VEL_Z_MOVING 100.0f // cm/s +// A landed multicopter should have near-zero vertical speed. Keep this independent +// from nav_land_detect_sensitivity so higher sensitivity cannot disarm during descent. +#define MC_LAND_DETECT_MAX_VEL_Z 50.0f // cm/s +// Only allow autonomous land detection near the configured final slow-descent phase. +#define MC_LAND_DETECT_DESCENT_DEMAND_MARGIN 25.0f // cm/s #define MC_LAND_THR_STABILISE_DELAY 1 // seconds #define MC_LAND_DESCEND_THROTTLE 40 // RC pwm units (us) #define MC_LAND_SAFE_SURFACE 5.0f // cm diff --git a/src/main/navigation/navigation_vtol_mc_protection.c b/src/main/navigation/navigation_vtol_mc_protection.c new file mode 100644 index 00000000000..65f00601390 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mc_protection.c @@ -0,0 +1,482 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" + +#include "fc/config.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/mixer_profile.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" +#include "navigation/navigation_vtol_mc_protection.h" + +#ifdef USE_AUTO_TRANSITION + +typedef enum { + VTOL_MC_PROTECT_FLAG_CONFIGURED = 1 << 0, + VTOL_MC_PROTECT_FLAG_VTOL_MC = 1 << 1, + VTOL_MC_PROTECT_FLAG_NAV_ACTIVE = 1 << 2, + VTOL_MC_PROTECT_FLAG_STABILIZED_ACTIVE = 1 << 3, + VTOL_MC_PROTECT_FLAG_CAPTURE_ACTIVE = 1 << 4, + VTOL_MC_PROTECT_FLAG_LANDING_SETTLE = 1 << 5, + VTOL_MC_PROTECT_FLAG_BAILOUT_ACTIVE = 1 << 6, + VTOL_MC_PROTECT_FLAG_RESERVE_SHRUNK = 1 << 7, + VTOL_MC_PROTECT_FLAG_SOFT_ALTITUDE = 1 << 8, + VTOL_MC_PROTECT_FLAG_COMMAND_SHAPED = 1 << 9, + VTOL_MC_PROTECT_FLAG_VELOCITY_FALLBACK = 1 << 10, +} vtolMcProtectionDebugFlags_e; + +typedef struct vtolMcProtectionRuntimeState_s { + vtolMcProtectionSettleState_t captureSettle; + vtolMcProtectionSettleState_t landingSettle; + vtolMcProtectionSettleState_t bailoutSettle; + bool captureActive; + bool softAltitudeActive; + bool landingSettleActive; + bool bailoutActive; + bool reserveShrunk; + bool stabilizedCommandShaped; + bool velocityFallbackActive; + timeMs_t bailoutStartMs; + int16_t bailoutStartThrottle; + uint16_t commandScalePermille; + int16_t safeThrottleMin; + int16_t safeThrottleMax; + int16_t protectedThrottle; +} vtolMcProtectionRuntimeState_t; + +static vtolMcProtectionRuntimeState_t vtolMcProtection = { + .commandScalePermille = 1000, +}; + +static bool navigationVtolMcProtectionConfigured(void) +{ + return (vtolMcProtectionMode_e)systemConfig()->vtolMcProtectionMode != VTOL_MC_PROTECTION_OFF; +} + +static bool navigationVtolMcProtectionPairedAirplaneProfileConfigured(void) +{ + for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) { + if (i != currentMixerProfileIndex && mixerConfigByIndex(i)->platformType == PLATFORM_AIRPLANE) { + return true; + } + } + + return platformTypeConfigured(PLATFORM_AIRPLANE); +} + +bool navigationVtolMcProtectionIsVtolMcMode(void) +{ + return vtolMcProtectionDetectVtolMcMode( + STATE(MULTIROTOR), + isMultirotorTypePlatform(currentMixerConfig.platformType), + navigationVtolMcProtectionPairedAirplaneProfileConfigured()); +} + +bool navigationVtolMcProtectionIsNavActive(void) +{ + return navigationVtolMcProtectionConfigured() && + ARMING_FLAG(ARMED) && + navigationVtolMcProtectionIsVtolMcMode(); +} + +bool navigationVtolMcProtectionVelocityUsable(void) +{ + return posControl.flags.estVelStatus == EST_TRUSTED; +} + +static bool navigationVtolMcProtectionVerticalVelocityUsable(void) +{ + return posControl.flags.estAltStatus >= EST_USABLE; +} + +uint16_t navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(void) +{ + const uint16_t absRoll = ABS(attitude.values.roll); + const uint16_t absPitch = ABS(attitude.values.pitch); + return absRoll > absPitch ? absRoll : absPitch; +} + +static uint16_t navigationVtolMcProtectionHorizontalSettleLimitCmS(void) +{ +#ifdef USE_MR_BRAKING_MODE + return vtolMcProtectionHorizontalSettleSpeedCmS(navConfig()->mc.braking_disengage_speed); +#else + return vtolMcProtectionHorizontalSettleSpeedCmS(0); +#endif +} + +static uint16_t navigationVtolMcProtectionVerticalSettleLimitCmS(void) +{ + return vtolMcProtectionVerticalSettleSpeedCmS(navConfig()->general.land_minalt_vspd); +} + +static uint16_t navigationVtolMcProtectionAttitudeSettleLimitDeciDeg(void) +{ + return vtolMcProtectionSettleAttitudeLimitDeciDeg(navConfig()->mc.max_bank_angle); +} + +static uint16_t navigationVtolMcProtectionBailoutLimitDeciDeg(void) +{ + return vtolMcProtectionBailoutAngleLimitDeciDeg(navConfig()->mc.max_bank_angle); +} + +static bool navigationVtolMcProtectionSettleConditionsMet(uint16_t *settleTimeMs) +{ + const bool horizontalVelocityUsable = navigationVtolMcProtectionVelocityUsable(); + const bool verticalVelocityUsable = navigationVtolMcProtectionVerticalVelocityUsable(); + + vtolMcProtection.velocityFallbackActive = vtolMcProtectionUsingVelocityFallback(horizontalVelocityUsable, verticalVelocityUsable); + *settleTimeMs = vtolMcProtectionSettleTimeMs(horizontalVelocityUsable); + + return vtolMcProtectionSettleConditionsMetWithFallback( + horizontalVelocityUsable, + verticalVelocityUsable, + posControl.actualState.velXY, + navGetCurrentActualPositionAndVelocity()->vel.z, + navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(), + navigationVtolMcProtectionHorizontalSettleLimitCmS(), + navigationVtolMcProtectionVerticalSettleLimitCmS(), + navigationVtolMcProtectionAttitudeSettleLimitDeciDeg()); +} + +static void navigationVtolMcProtectionPublishDebug(void) +{ + uint32_t flags = 0; + + if (navigationVtolMcProtectionConfigured()) { + flags |= VTOL_MC_PROTECT_FLAG_CONFIGURED; + } + if (navigationVtolMcProtectionIsVtolMcMode()) { + flags |= VTOL_MC_PROTECT_FLAG_VTOL_MC; + } + if (navigationVtolMcProtectionIsNavActive()) { + flags |= VTOL_MC_PROTECT_FLAG_NAV_ACTIVE; + } + if ((vtolMcProtectionMode_e)systemConfig()->vtolMcProtectionMode == VTOL_MC_PROTECTION_NAV_AND_STABILIZED && + ARMING_FLAG(ARMED) && navigationVtolMcProtectionIsVtolMcMode() && + (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + flags |= VTOL_MC_PROTECT_FLAG_STABILIZED_ACTIVE; + } + if (vtolMcProtection.captureActive) { + flags |= VTOL_MC_PROTECT_FLAG_CAPTURE_ACTIVE; + } + if (vtolMcProtection.landingSettleActive) { + flags |= VTOL_MC_PROTECT_FLAG_LANDING_SETTLE; + } + if (vtolMcProtection.bailoutActive) { + flags |= VTOL_MC_PROTECT_FLAG_BAILOUT_ACTIVE; + } + if (vtolMcProtection.reserveShrunk) { + flags |= VTOL_MC_PROTECT_FLAG_RESERVE_SHRUNK; + } + if (vtolMcProtection.softAltitudeActive) { + flags |= VTOL_MC_PROTECT_FLAG_SOFT_ALTITUDE; + } + if (vtolMcProtection.stabilizedCommandShaped) { + flags |= VTOL_MC_PROTECT_FLAG_COMMAND_SHAPED; + } + if (vtolMcProtection.velocityFallbackActive && + (vtolMcProtection.captureActive || vtolMcProtection.landingSettleActive || vtolMcProtection.bailoutActive)) { + flags |= VTOL_MC_PROTECT_FLAG_VELOCITY_FALLBACK; + } + + uint16_t debugProgress = 1000; + if (vtolMcProtection.captureActive) { + debugProgress = vtolMcProtection.captureSettle.elapsedMs; + } else if (vtolMcProtection.landingSettleActive) { + debugProgress = vtolMcProtection.landingSettle.elapsedMs; + } else if (vtolMcProtection.bailoutActive) { + debugProgress = vtolMcProtection.bailoutSettle.elapsedMs; + } else if (vtolMcProtection.stabilizedCommandShaped) { + debugProgress = vtolMcProtection.commandScalePermille; + } + + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 0, (int32_t)flags); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 1, vtolMcProtection.safeThrottleMin); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 2, vtolMcProtection.safeThrottleMax); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 3, vtolMcProtection.protectedThrottle); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 4, (int32_t)lrintf(posControl.actualState.velXY)); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 5, (int32_t)lrintf(navGetCurrentActualPositionAndVelocity()->vel.z)); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 6, navigationVtolMcProtectionMaxAbsAttitudeDeciDeg()); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 7, debugProgress); +} + +void navigationVtolMcProtectionResetTransientStates(void) +{ + vtolMcProtection.captureSettle.stableSinceMs = 0; + vtolMcProtection.captureSettle.elapsedMs = 0; + vtolMcProtection.landingSettle.stableSinceMs = 0; + vtolMcProtection.landingSettle.elapsedMs = 0; + vtolMcProtection.bailoutSettle.stableSinceMs = 0; + vtolMcProtection.bailoutSettle.elapsedMs = 0; + vtolMcProtection.captureActive = false; + vtolMcProtection.softAltitudeActive = false; + vtolMcProtection.landingSettleActive = false; + vtolMcProtection.bailoutActive = false; + vtolMcProtection.reserveShrunk = false; + vtolMcProtection.stabilizedCommandShaped = false; + vtolMcProtection.velocityFallbackActive = false; + vtolMcProtection.commandScalePermille = 1000; + vtolMcProtection.protectedThrottle = 0; +} + +vtolMcProtectionThrottleBounds_t navigationVtolMcProtectionGetThrottleBounds(const int16_t idleThrottle, const int16_t hoverThrottle, const int16_t maxThrottle) +{ + const bool active = navigationVtolMcProtectionIsNavActive() && navigationInAutomaticThrottleMode(); + vtolMcProtectionThrottleBounds_t bounds = vtolMcProtectionComputeThrottleBounds( + active, + idleThrottle, + hoverThrottle, + maxThrottle, + systemConfig()->vtolMcThrReservePercent); + + vtolMcProtection.safeThrottleMin = bounds.min; + vtolMcProtection.safeThrottleMax = bounds.max; + vtolMcProtection.reserveShrunk = bounds.reserveShrunk; + + if (!active) { + vtolMcProtection.bailoutActive = false; + vtolMcProtection.softAltitudeActive = false; + vtolMcProtection.velocityFallbackActive = false; + } + + navigationVtolMcProtectionPublishDebug(); + return bounds; +} + +static bool navigationVtolMcProtectionBailoutEntryCondition(void) +{ + return navigationVtolMcProtectionIsNavActive() && + navigationInAutomaticThrottleMode() && + navigationVtolMcProtectionMaxAbsAttitudeDeciDeg() >= navigationVtolMcProtectionBailoutLimitDeciDeg(); +} + +bool navigationVtolMcProtectionShouldFreezeAltitudeIntegrator(void) +{ + return vtolMcProtection.bailoutActive || + vtolMcProtection.softAltitudeActive || + navigationVtolMcProtectionBailoutEntryCondition(); +} + +static int16_t navigationVtolMcProtectionBailoutTargetThrottle(const int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, const int16_t hoverThrottle) +{ + int16_t targetThrottle = constrain(hoverThrottle, bounds->min, bounds->max); + const float verticalSpeedCmS = navGetCurrentActualPositionAndVelocity()->vel.z; + + if (verticalSpeedCmS < -(float)navigationVtolMcProtectionVerticalSettleLimitCmS() && targetThrottle < requestedThrottle) { + targetThrottle = requestedThrottle; + } + + return constrain(targetThrottle, bounds->min, bounds->max); +} + +int16_t navigationVtolMcProtectionApplyBailoutThrottle(const int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, const int16_t hoverThrottle) +{ + const bool entryCondition = navigationVtolMcProtectionBailoutEntryCondition(); + const timeMs_t nowMs = millis(); + + if (!entryCondition && vtolMcProtection.bailoutActive) { + uint16_t settleTimeMs; + const bool settleConditionsMet = navigationVtolMcProtectionSettleConditionsMet(&settleTimeMs); + const bool readyToExit = vtolMcProtectionUpdateSettleState( + &vtolMcProtection.bailoutSettle, + settleConditionsMet, + settleTimeMs, + nowMs); + + if (readyToExit) { + vtolMcProtection.bailoutActive = false; + vtolMcProtection.bailoutSettle.stableSinceMs = 0; + vtolMcProtection.bailoutSettle.elapsedMs = 0; + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + vtolMcProtection.protectedThrottle = requestedThrottle; + navigationVtolMcProtectionPublishDebug(); + return requestedThrottle; + } + } + + if (!entryCondition && !vtolMcProtection.bailoutActive) { + vtolMcProtection.protectedThrottle = requestedThrottle; + navigationVtolMcProtectionPublishDebug(); + return requestedThrottle; + } + + if (!vtolMcProtection.bailoutActive) { + vtolMcProtection.bailoutActive = true; + vtolMcProtection.bailoutStartMs = nowMs; + vtolMcProtection.bailoutStartThrottle = requestedThrottle; + vtolMcProtection.bailoutSettle.stableSinceMs = 0; + vtolMcProtection.bailoutSettle.elapsedMs = 0; + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + } + + const int16_t targetThrottle = navigationVtolMcProtectionBailoutTargetThrottle(requestedThrottle, bounds, hoverThrottle); + const uint16_t rampTimeMs = VTOL_MC_SETTLE_TIME_MS > VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS ? + VTOL_MC_SETTLE_TIME_MS : + VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS; + const float progress = constrainf((nowMs - vtolMcProtection.bailoutStartMs) / (float)rampTimeMs, 0.0f, 1.0f); + const int16_t protectedThrottle = lrintf(vtolMcProtection.bailoutStartThrottle + + (targetThrottle - vtolMcProtection.bailoutStartThrottle) * progress); + + vtolMcProtection.protectedThrottle = constrain(protectedThrottle, bounds->min, bounds->max); + navigationVtolMcProtectionPublishDebug(); + return vtolMcProtection.protectedThrottle; +} + +bool navigationVtolMcProtectionApplyCapture(const uint32_t navStateFlags) +{ + const bool captureAllowed = navigationVtolMcProtectionIsNavActive() && + (navStateFlags & NAV_CTL_POS) && + (navStateFlags & NAV_CTL_HOLD); + + if (!captureAllowed) { + vtolMcProtection.captureActive = false; + vtolMcProtection.captureSettle.stableSinceMs = 0; + vtolMcProtection.captureSettle.elapsedMs = 0; + navigationVtolMcProtectionPublishDebug(); + return false; + } + + uint16_t settleTimeMs; + const bool settleConditionsMet = navigationVtolMcProtectionSettleConditionsMet(&settleTimeMs); + const bool ready = vtolMcProtectionUpdateSettleState( + &vtolMcProtection.captureSettle, + settleConditionsMet, + settleTimeMs, + millis()); + + vtolMcProtection.captureActive = !ready; + navigationVtolMcProtectionPublishDebug(); + return vtolMcProtection.captureActive; +} + +bool navigationVtolMcProtectionApplySoftAltitudeCapture(const uint32_t navStateFlags) +{ + const bool holdOrTransitionState = ((navStateFlags & NAV_CTL_POS) && (navStateFlags & NAV_CTL_HOLD)) || + (navStateFlags & NAV_MIXERAT); + const bool shouldRelaxAltitude = vtolMcProtectionShouldRelaxAltitude( + navigationVtolMcProtectionIsNavActive(), + navigationInAutomaticThrottleMode(), + navigationVtolMcProtectionVelocityUsable(), + holdOrTransitionState, + navStateFlags & NAV_CTL_LAND, + navStateFlags & NAV_CTL_EMERG, + posControl.flags.isAdjustingAltitude, + posControl.actualState.velXY, + navigationVtolMcProtectionHorizontalSettleLimitCmS(), + vtolMcProtection.captureActive); + + vtolMcProtection.softAltitudeActive = shouldRelaxAltitude; + + if (shouldRelaxAltitude) { + // While a winged VTOL is bleeding horizontal speed, avoid chasing a stale + // altitude lock point; stabilize vertical rate and re-lock after settling. + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + } + + navigationVtolMcProtectionPublishDebug(); + return shouldRelaxAltitude; +} + +bool navigationVtolMcProtectionLandingSettleReady(const fpVector3_t *landingPos) +{ + if (!navigationVtolMcProtectionIsNavActive()) { + vtolMcProtection.landingSettleActive = false; + vtolMcProtection.landingSettle.stableSinceMs = 0; + vtolMcProtection.landingSettle.elapsedMs = 0; + navigationVtolMcProtectionPublishDebug(); + return true; + } + + const uint16_t landingCaptureRadiusCm = vtolMcProtectionLandingCaptureRadiusCm(navConfig()->general.waypoint_radius); + const bool insideLandingRadius = calculateDistanceToDestination(landingPos) <= landingCaptureRadiusCm; + uint16_t settleTimeMs; + const bool settleConditionsMet = navigationVtolMcProtectionSettleConditionsMet(&settleTimeMs); + const bool ready = vtolMcProtectionUpdateSettleState( + &vtolMcProtection.landingSettle, + insideLandingRadius && settleConditionsMet, + settleTimeMs, + millis()); + + vtolMcProtection.landingSettleActive = !ready; + navigationVtolMcProtectionPublishDebug(); + return ready; +} + +void navigationVtolMcProtectionApplyStabilizedCommandShaping(int16_t *rollCommand, int16_t *pitchCommand, int16_t *yawCommand) +{ + vtolMcProtection.stabilizedCommandShaped = false; + vtolMcProtection.commandScalePermille = 1000; + + if ((vtolMcProtectionMode_e)systemConfig()->vtolMcProtectionMode != VTOL_MC_PROTECTION_NAV_AND_STABILIZED || + !ARMING_FLAG(ARMED) || + !navigationVtolMcProtectionIsVtolMcMode() || + !(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) || + !navigationVtolMcProtectionVelocityUsable()) { + navigationVtolMcProtectionPublishDebug(); + return; + } + + const float commandScale = vtolMcProtectionCommandScaleForSpeed(posControl.actualState.velXY); + vtolMcProtection.commandScalePermille = vtolMcProtectionCommandScalePermille(commandScale); + + if (commandScale >= 0.999f) { + navigationVtolMcProtectionPublishDebug(); + return; + } + + const int16_t originalRoll = *rollCommand; + const int16_t originalPitch = *pitchCommand; + const int16_t originalYaw = *yawCommand; + + *rollCommand = vtolMcProtectionApplyCommandScale(*rollCommand, commandScale); + *pitchCommand = vtolMcProtectionApplyCommandScale(*pitchCommand, commandScale); + *yawCommand = vtolMcProtectionApplyCommandScale(*yawCommand, commandScale); + + vtolMcProtection.stabilizedCommandShaped = *rollCommand != originalRoll || + *pitchCommand != originalPitch || + *yawCommand != originalYaw; + navigationVtolMcProtectionPublishDebug(); +} + +void navigationVtolMcProtectionPublishThrottleDebug(const vtolMcProtectionThrottleBounds_t *bounds, const int16_t protectedThrottle) +{ + vtolMcProtection.safeThrottleMin = bounds->min; + vtolMcProtection.safeThrottleMax = bounds->max; + vtolMcProtection.reserveShrunk = bounds->reserveShrunk; + vtolMcProtection.protectedThrottle = protectedThrottle; + navigationVtolMcProtectionPublishDebug(); +} + +#endif diff --git a/src/main/navigation/navigation_vtol_mc_protection.h b/src/main/navigation/navigation_vtol_mc_protection.h new file mode 100644 index 00000000000..3d7c59b6498 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mc_protection.h @@ -0,0 +1,89 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#include "platform.h" + +#include "common/time.h" +#include "common/vector.h" + +#include "navigation/navigation_vtol_mc_protection_logic.h" + +#ifdef USE_AUTO_TRANSITION +bool navigationVtolMcProtectionIsVtolMcMode(void); +bool navigationVtolMcProtectionIsNavActive(void); +bool navigationVtolMcProtectionVelocityUsable(void); +uint16_t navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(void); + +vtolMcProtectionThrottleBounds_t navigationVtolMcProtectionGetThrottleBounds(int16_t idleThrottle, int16_t hoverThrottle, int16_t maxThrottle); +bool navigationVtolMcProtectionShouldFreezeAltitudeIntegrator(void); +int16_t navigationVtolMcProtectionApplyBailoutThrottle(int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, int16_t hoverThrottle); + +bool navigationVtolMcProtectionApplyCapture(uint32_t navStateFlags); +bool navigationVtolMcProtectionApplySoftAltitudeCapture(uint32_t navStateFlags); +bool navigationVtolMcProtectionLandingSettleReady(const fpVector3_t *landingPos); +void navigationVtolMcProtectionApplyStabilizedCommandShaping(int16_t *rollCommand, int16_t *pitchCommand, int16_t *yawCommand); +void navigationVtolMcProtectionPublishThrottleDebug(const vtolMcProtectionThrottleBounds_t *bounds, int16_t protectedThrottle); +void navigationVtolMcProtectionResetTransientStates(void); +#else +static inline bool navigationVtolMcProtectionIsVtolMcMode(void) { return false; } +static inline bool navigationVtolMcProtectionIsNavActive(void) { return false; } +static inline bool navigationVtolMcProtectionVelocityUsable(void) { return false; } +static inline uint16_t navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(void) { return 0; } +static inline vtolMcProtectionThrottleBounds_t navigationVtolMcProtectionGetThrottleBounds(int16_t idleThrottle, int16_t hoverThrottle, int16_t maxThrottle) +{ + return vtolMcProtectionComputeThrottleBounds(false, idleThrottle, hoverThrottle, maxThrottle, 0); +} +static inline bool navigationVtolMcProtectionShouldFreezeAltitudeIntegrator(void) { return false; } +static inline int16_t navigationVtolMcProtectionApplyBailoutThrottle(int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, int16_t hoverThrottle) +{ + (void)bounds; + (void)hoverThrottle; + return requestedThrottle; +} +static inline bool navigationVtolMcProtectionApplyCapture(uint32_t navStateFlags) +{ + (void)navStateFlags; + return false; +} +static inline bool navigationVtolMcProtectionApplySoftAltitudeCapture(uint32_t navStateFlags) +{ + (void)navStateFlags; + return false; +} +static inline bool navigationVtolMcProtectionLandingSettleReady(const fpVector3_t *landingPos) +{ + (void)landingPos; + return true; +} +static inline void navigationVtolMcProtectionApplyStabilizedCommandShaping(int16_t *rollCommand, int16_t *pitchCommand, int16_t *yawCommand) +{ + (void)rollCommand; + (void)pitchCommand; + (void)yawCommand; +} +static inline void navigationVtolMcProtectionPublishThrottleDebug(const vtolMcProtectionThrottleBounds_t *bounds, int16_t protectedThrottle) +{ + (void)bounds; + (void)protectedThrottle; +} +static inline void navigationVtolMcProtectionResetTransientStates(void) {} +#endif diff --git a/src/main/navigation/navigation_vtol_mc_protection_logic.h b/src/main/navigation/navigation_vtol_mc_protection_logic.h new file mode 100644 index 00000000000..a062f6a0b7a --- /dev/null +++ b/src/main/navigation/navigation_vtol_mc_protection_logic.h @@ -0,0 +1,261 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#include "common/time.h" + +#define VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM 100 +#define VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S 100 +#define VTOL_MC_SETTLE_ATTITUDE_CAP_DEG 20 +#define VTOL_MC_BAILOUT_ANGLE_EXTRA_DEG 15 +#define VTOL_MC_BAILOUT_ANGLE_MIN_DEG 45 +#define VTOL_MC_BAILOUT_ANGLE_MAX_DEG 60 +#define VTOL_MC_COMMAND_SHAPE_START_CM_S 300 +#define VTOL_MC_COMMAND_SHAPE_FULL_CM_S 800 +#define VTOL_MC_COMMAND_SHAPE_MIN_SCALE 0.5f +#define VTOL_MC_SETTLE_TIME_MS 1000 +#define VTOL_MC_FALLBACK_SETTLE_TIME_MS 3000 +#define VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS 500 +#define VTOL_MC_DEFAULT_BRAKING_DISENGAGE_CM_S 75 + +typedef struct vtolMcProtectionThrottleBounds_s { + int16_t min; + int16_t max; + bool reserveShrunk; +} vtolMcProtectionThrottleBounds_t; + +typedef struct vtolMcProtectionSettleState_s { + timeMs_t stableSinceMs; + uint16_t elapsedMs; +} vtolMcProtectionSettleState_t; + +static inline bool vtolMcProtectionDetectVtolMcMode( + const bool currentStateIsMultirotor, + const bool currentPlatformIsMultirotorType, + const bool pairedAirplaneProfileConfigured) +{ + return currentStateIsMultirotor && currentPlatformIsMultirotorType && pairedAirplaneProfileConfigured; +} + +static inline uint16_t vtolMcProtectionLandingCaptureRadiusCm(const uint16_t navWpRadiusCm) +{ + return navWpRadiusCm < VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM ? navWpRadiusCm : VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM; +} + +static inline uint16_t vtolMcProtectionHorizontalSettleSpeedCmS(const uint16_t brakingDisengageSpeedCmS) +{ + return brakingDisengageSpeedCmS > 0 ? brakingDisengageSpeedCmS : VTOL_MC_DEFAULT_BRAKING_DISENGAGE_CM_S; +} + +static inline uint16_t vtolMcProtectionVerticalSettleSpeedCmS(const uint16_t landMinAltVspdCmS) +{ + return landMinAltVspdCmS < VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S ? landMinAltVspdCmS : VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S; +} + +static inline uint16_t vtolMcProtectionSettleAttitudeLimitDeciDeg(const uint8_t navMcBankAngleDeg) +{ + const uint8_t limitedAngleDeg = navMcBankAngleDeg < VTOL_MC_SETTLE_ATTITUDE_CAP_DEG ? navMcBankAngleDeg : VTOL_MC_SETTLE_ATTITUDE_CAP_DEG; + return (uint16_t)limitedAngleDeg * 10U; +} + +static inline uint16_t vtolMcProtectionBailoutAngleLimitDeciDeg(const uint8_t navMcBankAngleDeg) +{ + uint8_t bailoutAngleDeg = navMcBankAngleDeg + VTOL_MC_BAILOUT_ANGLE_EXTRA_DEG; + + if (bailoutAngleDeg < VTOL_MC_BAILOUT_ANGLE_MIN_DEG) { + bailoutAngleDeg = VTOL_MC_BAILOUT_ANGLE_MIN_DEG; + } else if (bailoutAngleDeg > VTOL_MC_BAILOUT_ANGLE_MAX_DEG) { + bailoutAngleDeg = VTOL_MC_BAILOUT_ANGLE_MAX_DEG; + } + + return (uint16_t)bailoutAngleDeg * 10U; +} + +static inline vtolMcProtectionThrottleBounds_t vtolMcProtectionComputeThrottleBounds( + const bool active, + const int16_t idleThrottle, + const int16_t hoverThrottle, + const int16_t maxThrottle, + const uint8_t reservePercent) +{ + vtolMcProtectionThrottleBounds_t bounds = { + .min = idleThrottle, + .max = maxThrottle, + .reserveShrunk = false, + }; + + if (!active || reservePercent == 0 || maxThrottle <= idleThrottle) { + return bounds; + } + + const int16_t throttleRange = maxThrottle - idleThrottle; + const int16_t requestedReserve = (throttleRange * reservePercent) / 100; + const int16_t lowReserveLimit = hoverThrottle > idleThrottle ? hoverThrottle - idleThrottle : 0; + const int16_t highReserveLimit = maxThrottle > hoverThrottle ? maxThrottle - hoverThrottle : 0; + const int16_t lowReserve = requestedReserve < lowReserveLimit ? requestedReserve : lowReserveLimit; + const int16_t highReserve = requestedReserve < highReserveLimit ? requestedReserve : highReserveLimit; + + bounds.min = idleThrottle + lowReserve; + bounds.max = maxThrottle - highReserve; + bounds.reserveShrunk = lowReserve != requestedReserve || highReserve != requestedReserve; + + return bounds; +} + +static inline bool vtolMcProtectionSettleConditionsMet( + const float horizontalSpeedCmS, + const float verticalSpeedCmS, + const uint16_t maxAbsAttitudeDeciDeg, + const uint16_t horizontalLimitCmS, + const uint16_t verticalLimitCmS, + const uint16_t attitudeLimitDeciDeg) +{ + const float absVerticalSpeedCmS = verticalSpeedCmS < 0.0f ? -verticalSpeedCmS : verticalSpeedCmS; + + return horizontalSpeedCmS <= horizontalLimitCmS && + absVerticalSpeedCmS <= verticalLimitCmS && + maxAbsAttitudeDeciDeg <= attitudeLimitDeciDeg; +} + +static inline bool vtolMcProtectionUsingVelocityFallback( + const bool horizontalVelocityUsable, + const bool verticalVelocityUsable) +{ + return !horizontalVelocityUsable && verticalVelocityUsable; +} + +static inline uint16_t vtolMcProtectionSettleTimeMs(const bool horizontalVelocityUsable) +{ + return horizontalVelocityUsable ? VTOL_MC_SETTLE_TIME_MS : VTOL_MC_FALLBACK_SETTLE_TIME_MS; +} + +static inline bool vtolMcProtectionSettleConditionsMetWithFallback( + const bool horizontalVelocityUsable, + const bool verticalVelocityUsable, + const float horizontalSpeedCmS, + const float verticalSpeedCmS, + const uint16_t maxAbsAttitudeDeciDeg, + const uint16_t horizontalLimitCmS, + const uint16_t verticalLimitCmS, + const uint16_t attitudeLimitDeciDeg) +{ + if (!verticalVelocityUsable) { + return false; + } + + if (horizontalVelocityUsable) { + return vtolMcProtectionSettleConditionsMet( + horizontalSpeedCmS, + verticalSpeedCmS, + maxAbsAttitudeDeciDeg, + horizontalLimitCmS, + verticalLimitCmS, + attitudeLimitDeciDeg); + } + + const float absVerticalSpeedCmS = verticalSpeedCmS < 0.0f ? -verticalSpeedCmS : verticalSpeedCmS; + return absVerticalSpeedCmS <= verticalLimitCmS && + maxAbsAttitudeDeciDeg <= attitudeLimitDeciDeg; +} + +static inline bool vtolMcProtectionUpdateSettleState( + vtolMcProtectionSettleState_t *state, + const bool conditionsMet, + const uint16_t settleTimeMs, + const timeMs_t nowMs) +{ + if (!conditionsMet) { + state->stableSinceMs = 0; + state->elapsedMs = 0; + return false; + } + + if (settleTimeMs == 0) { + state->stableSinceMs = nowMs; + state->elapsedMs = 0; + return true; + } + + if (state->stableSinceMs == 0) { + state->stableSinceMs = nowMs; + state->elapsedMs = 0; + return false; + } + + const timeMs_t elapsedMs = nowMs - state->stableSinceMs; + state->elapsedMs = elapsedMs > UINT16_MAX ? UINT16_MAX : elapsedMs; + return elapsedMs >= settleTimeMs; +} + +static inline bool vtolMcProtectionShouldRelaxAltitude( + const bool active, + const bool automaticThrottle, + const bool velocityUsable, + const bool holdOrTransitionState, + const bool landingState, + const bool emergencyState, + const bool altitudeStickActive, + const float horizontalSpeedCmS, + const uint16_t horizontalLimitCmS, + const bool captureActive) +{ + return active && + automaticThrottle && + holdOrTransitionState && + !landingState && + !emergencyState && + !altitudeStickActive && + (captureActive || (velocityUsable && horizontalSpeedCmS > horizontalLimitCmS)); +} + +static inline float vtolMcProtectionCommandScaleForSpeed(const float horizontalSpeedCmS) +{ + if (horizontalSpeedCmS <= VTOL_MC_COMMAND_SHAPE_START_CM_S) { + return 1.0f; + } + + if (horizontalSpeedCmS >= VTOL_MC_COMMAND_SHAPE_FULL_CM_S) { + return VTOL_MC_COMMAND_SHAPE_MIN_SCALE; + } + + const float progress = (horizontalSpeedCmS - VTOL_MC_COMMAND_SHAPE_START_CM_S) / + (float)(VTOL_MC_COMMAND_SHAPE_FULL_CM_S - VTOL_MC_COMMAND_SHAPE_START_CM_S); + return 1.0f - progress * (1.0f - VTOL_MC_COMMAND_SHAPE_MIN_SCALE); +} + +static inline uint16_t vtolMcProtectionCommandScalePermille(const float commandScale) +{ + return (uint16_t)(commandScale * 1000.0f + 0.5f); +} + +static inline int16_t vtolMcProtectionApplyCommandScale(const int16_t command, const float commandScale) +{ + if (command == 0 || commandScale >= 0.999f) { + return command; + } + + int16_t shapedCommand = (int16_t)(command * commandScale + (command > 0 ? 0.5f : -0.5f)); + if (shapedCommand == 0) { + shapedCommand = command > 0 ? 1 : -1; + } + + return shapedCommand; +} diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 8aa20681d2c..98c3838f687 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -72,6 +72,7 @@ set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GI set_property(SOURCE mixer_transition_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) set_property(SOURCE mixer_transition_policy_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) set_property(SOURCE mixer_transition_scenarios_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE vtol_mc_protection_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) function(unit_test src) get_filename_component(basename ${src} NAME) diff --git a/src/test/unit/vtol_mc_protection_logic_unittest.cc b/src/test/unit/vtol_mc_protection_logic_unittest.cc new file mode 100644 index 00000000000..10f880b2491 --- /dev/null +++ b/src/test/unit/vtol_mc_protection_logic_unittest.cc @@ -0,0 +1,214 @@ +#include + +extern "C" { +#include "navigation/navigation_vtol_mc_protection_logic.h" +} + +TEST(VtolMcProtectionLogicTest, DetectsOnlyPairedMultirotorVtolMcMode) +{ + EXPECT_TRUE(vtolMcProtectionDetectVtolMcMode(true, true, true)); + EXPECT_FALSE(vtolMcProtectionDetectVtolMcMode(false, true, true)); + EXPECT_FALSE(vtolMcProtectionDetectVtolMcMode(true, false, true)); + EXPECT_FALSE(vtolMcProtectionDetectVtolMcMode(true, true, false)); +} + +TEST(VtolMcProtectionLogicTest, DefaultOffKeepsLegacyThrottleBounds) +{ + const vtolMcProtectionThrottleBounds_t bounds = vtolMcProtectionComputeThrottleBounds( + false, + 1000, + 1500, + 2000, + 15); + + EXPECT_EQ(1000, bounds.min); + EXPECT_EQ(2000, bounds.max); + EXPECT_FALSE(bounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, AppliesThrottleReserveBeforePidBounds) +{ + const vtolMcProtectionThrottleBounds_t bounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1500, + 2000, + 15); + + EXPECT_EQ(1150, bounds.min); + EXPECT_EQ(1850, bounds.max); + EXPECT_FALSE(bounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, ShrinksReserveToKeepHoverThrottleReachable) +{ + const vtolMcProtectionThrottleBounds_t lowHoverBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1100, + 2000, + 20); + + EXPECT_EQ(1100, lowHoverBounds.min); + EXPECT_EQ(1800, lowHoverBounds.max); + EXPECT_TRUE(lowHoverBounds.reserveShrunk); + + const vtolMcProtectionThrottleBounds_t highHoverBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1900, + 2000, + 20); + + EXPECT_EQ(1200, highHoverBounds.min); + EXPECT_EQ(1900, highHoverBounds.max); + EXPECT_TRUE(highHoverBounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, ThrottleBoundsHandleDisabledReserveAndInvalidRange) +{ + const vtolMcProtectionThrottleBounds_t zeroReserveBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1500, + 2000, + 0); + + EXPECT_EQ(1000, zeroReserveBounds.min); + EXPECT_EQ(2000, zeroReserveBounds.max); + EXPECT_FALSE(zeroReserveBounds.reserveShrunk); + + const vtolMcProtectionThrottleBounds_t invalidRangeBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1500, + 1500, + 1400, + 20); + + EXPECT_EQ(1500, invalidRangeBounds.min); + EXPECT_EQ(1400, invalidRangeBounds.max); + EXPECT_FALSE(invalidRangeBounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, SettleTimerRequiresContinuousStableConditions) +{ + vtolMcProtectionSettleState_t state = {}; + + EXPECT_EQ(1000, VTOL_MC_SETTLE_TIME_MS); + EXPECT_EQ(3000, VTOL_MC_FALLBACK_SETTLE_TIME_MS); + EXPECT_EQ(VTOL_MC_SETTLE_TIME_MS, vtolMcProtectionSettleTimeMs(true)); + EXPECT_EQ(VTOL_MC_FALLBACK_SETTLE_TIME_MS, vtolMcProtectionSettleTimeMs(false)); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 100)); + EXPECT_EQ(0, state.elapsedMs); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 600)); + EXPECT_EQ(500, state.elapsedMs); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, false, VTOL_MC_SETTLE_TIME_MS, 700)); + EXPECT_EQ(0, state.elapsedMs); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 800)); + EXPECT_TRUE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 1800)); +} + +TEST(VtolMcProtectionLogicTest, SettleTimerSaturatesElapsedTime) +{ + vtolMcProtectionSettleState_t state = {}; + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, UINT16_MAX, 1)); + EXPECT_TRUE(vtolMcProtectionUpdateSettleState(&state, true, UINT16_MAX, 70000)); + EXPECT_EQ(UINT16_MAX, state.elapsedMs); +} + +TEST(VtolMcProtectionLogicTest, SettleConditionsUseDerivedThresholds) +{ + EXPECT_EQ(75, vtolMcProtectionHorizontalSettleSpeedCmS(0)); + EXPECT_EQ(50, vtolMcProtectionHorizontalSettleSpeedCmS(50)); + EXPECT_EQ(50, vtolMcProtectionVerticalSettleSpeedCmS(50)); + EXPECT_EQ(100, vtolMcProtectionVerticalSettleSpeedCmS(500)); + EXPECT_EQ(200, vtolMcProtectionSettleAttitudeLimitDeciDeg(35)); + EXPECT_EQ(150, vtolMcProtectionSettleAttitudeLimitDeciDeg(15)); + + EXPECT_TRUE(vtolMcProtectionSettleConditionsMet(70.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMet(80.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMet(70.0f, -120.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMet(70.0f, -50.0f, 250, 75, 100, 200)); +} + +TEST(VtolMcProtectionLogicTest, VelocityFallbackSettleUsesVerticalSpeedAndAttitudeOnly) +{ + EXPECT_FALSE(vtolMcProtectionUsingVelocityFallback(true, true)); + EXPECT_FALSE(vtolMcProtectionUsingVelocityFallback(false, false)); + EXPECT_TRUE(vtolMcProtectionUsingVelocityFallback(false, true)); + + EXPECT_TRUE(vtolMcProtectionSettleConditionsMetWithFallback( + false, true, 500.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + false, false, 0.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + false, true, 0.0f, -120.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + false, true, 0.0f, -50.0f, 250, 75, 100, 200)); + + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + true, true, 500.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + true, false, 0.0f, 0.0f, 0, 75, 100, 200)); +} + +TEST(VtolMcProtectionLogicTest, LandingCaptureRadiusCapsLargeWaypointRadius) +{ + EXPECT_EQ(100, vtolMcProtectionLandingCaptureRadiusCm(10000)); + EXPECT_EQ(80, vtolMcProtectionLandingCaptureRadiusCm(80)); +} + +TEST(VtolMcProtectionLogicTest, SoftAltitudeRelaxationOnlyAppliesDuringCaptureOrTransition) +{ + EXPECT_TRUE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, false, 120.0f, 75, false)); + EXPECT_TRUE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, false, 20.0f, 75, true)); + + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + false, true, true, true, false, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, false, true, true, false, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, false, true, false, false, false, 120.0f, 75, false)); + EXPECT_TRUE(vtolMcProtectionShouldRelaxAltitude( + true, true, false, true, false, false, false, 120.0f, 75, true)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, false, false, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, true, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, true, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, true, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, false, 75.0f, 75, false)); +} + +TEST(VtolMcProtectionLogicTest, BailoutAngleLimitUsesBankAngleWithSafeClamps) +{ + EXPECT_EQ(450, vtolMcProtectionBailoutAngleLimitDeciDeg(20)); + EXPECT_EQ(500, vtolMcProtectionBailoutAngleLimitDeciDeg(35)); + EXPECT_EQ(600, vtolMcProtectionBailoutAngleLimitDeciDeg(80)); +} + +TEST(VtolMcProtectionLogicTest, CommandShapingIsContinuousAndPreservesSign) +{ + EXPECT_FLOAT_EQ(1.0f, vtolMcProtectionCommandScaleForSpeed(250.0f)); + EXPECT_FLOAT_EQ(1.0f, vtolMcProtectionCommandScaleForSpeed(VTOL_MC_COMMAND_SHAPE_START_CM_S)); + EXPECT_FLOAT_EQ(0.75f, vtolMcProtectionCommandScaleForSpeed(550.0f)); + EXPECT_FLOAT_EQ(0.5f, vtolMcProtectionCommandScaleForSpeed(VTOL_MC_COMMAND_SHAPE_FULL_CM_S)); + EXPECT_FLOAT_EQ(0.5f, vtolMcProtectionCommandScaleForSpeed(900.0f)); + EXPECT_EQ(750, vtolMcProtectionCommandScalePermille(0.75f)); + + EXPECT_EQ(0, vtolMcProtectionApplyCommandScale(0, 0.5f)); + EXPECT_EQ(150, vtolMcProtectionApplyCommandScale(200, 0.75f)); + EXPECT_EQ(-150, vtolMcProtectionApplyCommandScale(-200, 0.75f)); + EXPECT_EQ(1, vtolMcProtectionApplyCommandScale(1, 0.5f)); + EXPECT_EQ(-1, vtolMcProtectionApplyCommandScale(-1, 0.5f)); +} From b8360ba3eff54725c282d0c290f33c8eb938a7b0 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Tue, 16 Jun 2026 17:47:33 +0300 Subject: [PATCH 34/42] fix tilt servos oposite movment after direct switch add servo handoff hold --- src/main/flight/mixer_profile.c | 83 ++++++++++++++++--- src/main/flight/mixer_profile.h | 2 + src/main/flight/mixer_transition_logic.h | 5 ++ .../unit/mixer_transition_logic_unittest.cc | 8 ++ 4 files changed, 88 insertions(+), 10 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 38fea4b1eda..eaf731afd42 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -36,6 +36,10 @@ #include "common/log.h" #include "build/debug.h" +#ifdef USE_AUTO_TRANSITION +#define MIXER_TRANSITION_DIRECT_SWITCH_SERVO_HOLD_MS 300 +#endif + mixerConfig_t currentMixerConfig; int currentMixerProfileIndex; bool isMixerTransitionMixing; @@ -160,6 +164,12 @@ void setMixerProfileAT(void) mixerProfileAT.postSwitchFadeDurationMs = 0; mixerProfileAT.postSwitchFadeStartTime = 0; memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); + mixerProfileAT.servoHandoffMask = 0; + mixerProfileAT.servoHandoffDurationMs = 0; + mixerProfileAT.servoHandoffHoldDurationMs = 0; + mixerProfileAT.servoHandoffStartTime = 0; + mixerProfileAT.servoHandoffHoldStartTime = 0; + memset(mixerProfileAT.servoHandoffOutput, 0, sizeof(mixerProfileAT.servoHandoffOutput)); #else mixerProfileAT.transitionStartTime = millis(); mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; @@ -171,7 +181,9 @@ static void clearServoHandoffFade(void) { mixerProfileAT.servoHandoffMask = 0; mixerProfileAT.servoHandoffDurationMs = 0; + mixerProfileAT.servoHandoffHoldDurationMs = 0; mixerProfileAT.servoHandoffStartTime = 0; + mixerProfileAT.servoHandoffHoldStartTime = 0; memset(mixerProfileAT.servoHandoffOutput, 0, sizeof(mixerProfileAT.servoHandoffOutput)); } @@ -332,12 +344,36 @@ static void prepareServoHandoffFade(const uint32_t handoffMask) } } +static void prepareServoHandoffHold(const uint32_t handoffMask, const uint16_t holdDurationMs) +{ + clearServoHandoffFade(); + + if (handoffMask == 0 || holdDurationMs == 0) { + return; + } + + mixerProfileAT.servoHandoffMask = handoffMask; + mixerProfileAT.servoHandoffDurationMs = getServoHandoffDurationMs(); + mixerProfileAT.servoHandoffHoldDurationMs = holdDurationMs; + mixerProfileAT.servoHandoffHoldStartTime = millis(); + + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + if ((handoffMask & (1U << i)) == 0) { + continue; + } + + mixerProfileAT.servoHandoffOutput[i] = servo[i]; + } +} + static void startServoHandoffFade(void) { if (mixerProfileAT.servoHandoffMask == 0 || mixerProfileAT.servoHandoffDurationMs == 0) { return; } + mixerProfileAT.servoHandoffHoldDurationMs = 0; + mixerProfileAT.servoHandoffHoldStartTime = 0; mixerProfileAT.servoHandoffStartTime = millis(); } @@ -623,13 +659,16 @@ static void updateTransitionScales(void) mixerProfileAT.fwAuthorityScale = scales.fwAuthorityScale; } -static void abortTransition(const bool byAirspeedTimeout) +static void abortTransition(const bool byAirspeedTimeout, const bool holdServoOutputsForDirectSwitch) { const bool wasActive = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; const bool hotSwitchAlreadyDone = mixerProfileAT.hotSwitchDone; + const uint32_t servoHandoffMask = (wasActive && !hotSwitchAlreadyDone) ? + collectServoHandoffMask(nextMixerProfileIndex, false) : + 0; - if (wasActive && !hotSwitchAlreadyDone) { - prepareServoHandoffFade(collectServoHandoffMask(nextMixerProfileIndex, false)); + if (servoHandoffMask != 0 && !holdServoOutputsForDirectSwitch) { + prepareServoHandoffFade(servoHandoffMask); } isMixerTransitionMixing_requested = false; @@ -644,7 +683,13 @@ static void abortTransition(const bool byAirspeedTimeout) mixerProfileAT.transitionStartAirspeedCmS = 0.0f; resetTransitionScales(); - if (wasActive && !hotSwitchAlreadyDone) { + if (servoHandoffMask != 0 && holdServoOutputsForDirectSwitch) { + // A manual direct switch may report MIXERTRANSITION OFF a few control + // cycles before MIXERPROFILE changes. Hold the real servo output so + // transition-linked tilt servos do not briefly move back toward the + // source profile before the destination profile takes ownership. + prepareServoHandoffHold(servoHandoffMask, MIXER_TRANSITION_DIRECT_SWITCH_SERVO_HOLD_MS); + } else if (servoHandoffMask != 0) { startServoHandoffFade(); } } @@ -747,7 +792,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) updatePostSwitchFade(); return true; } - abortTransition(false); + abortTransition(false, false); return true; } switch (mixerProfileAT.phase) { @@ -772,7 +817,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) case MIXERAT_PHASE_TRANSITIONING: isMixerTransitionMixing_requested = true; if (required_action != MIXERAT_REQUEST_NONE && required_action != mixerProfileAT.request) { - abortTransition(false); + abortTransition(false, false); return true; } @@ -783,7 +828,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) prepareServoHandoffFade(collectServoHandoffMask(nextMixerProfileIndex, true)); preparePostSwitchFade(nextMixerProfileIndex); if (!outputProfileHotSwitch(nextMixerProfileIndex)) { - abortTransition(false); + abortTransition(false, false); return true; } mixerProfileAT.hotSwitchDone = true; @@ -797,7 +842,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) } else if (mixerProfileAT.usedAirspeed && currentMixerConfig.vtolTransitionAirspeedTimeoutMs > 0 && (millis() - mixerProfileAT.transitionStartTime) >= currentMixerConfig.vtolTransitionAirspeedTimeoutMs) { - abortTransition(true); + abortTransition(true, false); return true; } @@ -911,7 +956,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) } if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { - abortTransition(false); + abortTransition(false, false); manualTransitionSessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; manualFwToMcProtectionLatched = false; mixerAT_inuse = false; @@ -981,7 +1026,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) mixerAT_inuse && !mixerProfileAT.hotSwitchDone && (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { - abortTransition(false); + abortTransition(false, true); mixerAT_inuse = false; } @@ -1189,6 +1234,24 @@ bool mixerATGetServoHandoffOutput(uint8_t servoIndex, int16_t currentOutput, int return false; } + if (mixerProfileAT.servoHandoffHoldDurationMs > 0) { + const uint32_t holdElapsedMs = millis() - mixerProfileAT.servoHandoffHoldStartTime; + if (mixerTransitionServoHandoffHoldActive(mixerProfileAT.servoHandoffHoldDurationMs, holdElapsedMs)) { + *output = mixerProfileAT.servoHandoffOutput[servoIndex]; + return true; + } + + mixerProfileAT.servoHandoffHoldDurationMs = 0; + mixerProfileAT.servoHandoffHoldStartTime = 0; + + if (mixerProfileAT.servoHandoffDurationMs == 0) { + clearServoHandoffFade(); + return false; + } + + mixerProfileAT.servoHandoffStartTime = millis(); + } + const float progress = mixerProfileAT.servoHandoffDurationMs == 0 ? 1.0f : constrainf((float)(millis() - mixerProfileAT.servoHandoffStartTime) / (float)mixerProfileAT.servoHandoffDurationMs, 0.0f, 1.0f); diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 92e1cedfc50..a3f301e2e00 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -98,8 +98,10 @@ typedef struct mixerProfileAT_s { timeMs_t postSwitchFadeStartTime; uint32_t servoHandoffMask; uint16_t servoHandoffDurationMs; + uint16_t servoHandoffHoldDurationMs; int16_t servoHandoffOutput[MAX_SUPPORTED_SERVOS]; timeMs_t servoHandoffStartTime; + timeMs_t servoHandoffHoldStartTime; timeMs_t transitionStartTime; #else bool transitionInputMixing; diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h index 21de8d8366b..af0ec5e7b70 100644 --- a/src/main/flight/mixer_transition_logic.h +++ b/src/main/flight/mixer_transition_logic.h @@ -138,6 +138,11 @@ static inline uint16_t mixerTransitionComputeServoHandoffDurationMs( return scaleRampTimeMs; } +static inline bool mixerTransitionServoHandoffHoldActive(uint16_t holdDurationMs, uint32_t elapsedMs) +{ + return holdDurationMs > 0 && elapsedMs < holdDurationMs; +} + static inline float mixerTransitionResolveHandoffProgress( bool dynamicMixerEnabled, bool usedAirspeed, diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc index c2e77afa929..6b0d217a00c 100644 --- a/src/test/unit/mixer_transition_logic_unittest.cc +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -254,6 +254,14 @@ TEST(MixerTransitionLogicTest, ServoHandoffUsesConfiguredScaleRampWhenDynamicMix EXPECT_EQ(0, mixerTransitionComputeServoHandoffDurationMs(false, 0, 750)); } +TEST(MixerTransitionLogicTest, DirectSwitchServoHoldIsIndependentFromHandoffDuration) +{ + EXPECT_TRUE(mixerTransitionServoHandoffHoldActive(300, 0)); + EXPECT_TRUE(mixerTransitionServoHandoffHoldActive(300, 299)); + EXPECT_FALSE(mixerTransitionServoHandoffHoldActive(300, 300)); + EXPECT_FALSE(mixerTransitionServoHandoffHoldActive(0, 0)); +} + TEST(MixerTransitionLogicTest, ServoHandoffBlendStartsFromCapturedOutputAfterHotSwitch) { EXPECT_EQ(1366, mixerTransitionBlendCapturedServoOutput(1366, 980, 0.0f)); From 39c8f165160cd8d645e62d05c25ddfb531dc9c6b Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Wed, 17 Jun 2026 12:11:43 +0300 Subject: [PATCH 35/42] Improve VTOL auto-transition safety and documentation - Add VTOL mission transition precondition handling and tests - Add VTOL MC protection logic for throttle reserve, settle checks, command shaping, bailout, and landing confirmation - Improve VTOL MC landing detection with throttle-probe confirmation and safer bump handling - Document manual and mission VTOL auto-transition setup, dynamic scaling, pusher and tilt-servo examples - Clarify related VTOL, navigation, landing, airspeed timeout, retry, and fail-action settings --- docs/MixerProfile.md | 74 +- docs/Navigation.md | 50 +- docs/Settings.md | 58 +- docs/VTOL.md | 1052 +++++++++++------ src/main/fc/settings.yaml | 36 +- src/main/navigation/navigation.c | 29 +- src/main/navigation/navigation_multicopter.c | 145 ++- src/main/navigation/navigation_private.h | 2 + .../navigation_vtol_mc_protection_logic.h | 99 ++ .../navigation_vtol_mission_logic.h | 51 + src/test/unit/CMakeLists.txt | 1 + .../navigation_vtol_mission_logic_unittest.cc | 113 ++ .../unit/vtol_mc_protection_logic_unittest.cc | 177 +++ 13 files changed, 1465 insertions(+), 422 deletions(-) create mode 100644 src/main/navigation/navigation_vtol_mission_logic.h create mode 100644 src/test/unit/navigation_vtol_mission_logic_unittest.cc diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 23a93e8ebb3..8cac8b1b812 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -31,7 +31,7 @@ Mapping a motor to a servo output, or using servo logic conditions for this feat If `mixer_vtol_manualswitch_autotransition_controller = ON`, `MIXER TRANSITION` works like a start switch for one transition: -- Each time you move `MIXER TRANSITION` from OFF to ON, iNAV starts one transition. +- Each time you move `MIXER TRANSITION` from OFF to ON, INAV starts one transition. - The same switch can be used in both directions: - MC -> transition -> FW - FW -> transition -> MC @@ -39,7 +39,7 @@ If `mixer_vtol_manualswitch_autotransition_controller = ON`, `MIXER TRANSITION` - Leaving the switch ON does not keep restarting the transition. - To start another transition, turn the switch OFF and then ON again. - If you turn the switch OFF before the profile change happens, that transition request is cancelled. -- Optional extra protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can automatically start FW->MC if airspeed gets too low. After that switch, iNAV stays in MC until you deliberately command another manual profile change. +- Optional extra protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can automatically start FW->MC if airspeed gets too low. After that switch, INAV stays in MC until you deliberately command another manual profile change. This behavior is controlled by `mixer_vtol_manualswitch_autotransition_controller`. Turn it ON in both mixer profiles if you want the same switch behavior in both directions. @@ -55,7 +55,7 @@ There are two separate manual paths: - `MIXER PROFILE 2` is still a direct manual profile switch when `MIXER TRANSITION` is OFF. - `MIXER TRANSITION` starts the smooth automatic transition sequence when `mixer_vtol_manualswitch_autotransition_controller = ON`. - If both are ON together while the automatic transition controller is enabled, the controller temporarily owns the profile switching. When `MIXER TRANSITION` turns OFF again, direct `MIXER PROFILE 2` switching becomes active again. -- If low-speed protection switches the model from FW to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. +- If low-speed protection switches the model from FW to MC, INAV keeps the MC profile until you deliberately command another manual profile change. 3-position switch example: @@ -130,7 +130,7 @@ This overlap style is supported too. This feature is mainly used for RTH and failsafe. When set up correctly, the aircraft can use fixed-wing for the efficient flight home and then return to multicopter for easier landing. -If `mixer_automated_switch = ON` in a mixer profile, iNAV is allowed to run an automated transition when the navigation logic asks for it. +If `mixer_automated_switch = ON` in a mixer profile, INAV is allowed to run an automated transition when the navigation logic asks for it. - Use `mixer_automated_switch = ON` in the MC mixer profile if you want automated MC->FW transition during the head-home part of RTH. - Use `mixer_automated_switch = ON` in the FW mixer profile if you want automated FW->MC transition for the landing part. @@ -149,24 +149,24 @@ Direct manual `MIXER PROFILE 2` switching is still a separate path when you want ### Airspeed-first completion -When valid pitot airspeed is available, iNAV uses airspeed to decide when the transition is complete: +When valid pitot airspeed is available, INAV uses airspeed to decide when the transition is complete: - `vtol_transition_to_fw_min_airspeed_cm_s` for MC->FW - `vtol_transition_to_mc_max_airspeed_cm_s` for FW->MC -If pitot is not available, not healthy, or the threshold is set to `0`, iNAV uses `mixer_switch_trans_timer` instead. +If pitot is not available, not healthy, or the threshold is set to `0`, INAV uses `mixer_switch_trans_timer` instead. Ground speed is not used for transition completion/progress. The three timer settings do different jobs: - `mixer_switch_trans_timer` is the original VTOL transition timer. It is still the backup completion timer when trusted pitot airspeed is not being used. - `mixer_vtol_transition_airspeed_timeout_ms` is only a maximum wait time for the required airspeed while pitot is still usable. It does not complete the transition by itself; it aborts that airspeed-controlled attempt. -- If pitot becomes unavailable during transition, iNAV stops using the airspeed timeout and falls back to `mixer_switch_trans_timer`. +- If pitot becomes unavailable during transition, INAV stops using the airspeed timeout and falls back to `mixer_switch_trans_timer`. - For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). ### Smooth power changes during transition (optional) -When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: +When `mixer_vtol_transition_dynamic_mixer = ON`, INAV can smoothly change: - forward motor power, - lift motor power, @@ -175,39 +175,41 @@ When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: Default is OFF to preserve existing behavior. With it ON, you can configure `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules in the MC mixer profile. -During MC->FW they give those servos a preview of the fixed-wing stabilisation that will take over after the hot-switch. +During MC->FW they give those servos a preview of the fixed-wing stabilisation that will take over after the profile switch. This preview uses the target fixed-wing PID bank, rates, angle limits, heading-hold limits, and turn-assist gains, but it still follows the current transition stick shaping until the actual profile switch. -During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. -At the same time, target MC motor rules can fade in before the switch and use a target MC PID preview, so lift motors are not driven by the active FW/PIFF controller. -These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. -`INPUT_MIXER_TRANSITION` remains available for tilt/helper servo movement. With `mixer_vtol_transition_dynamic_mixer = ON`, it follows `mixer_vtol_transition_scale_ramp_time_ms`. With it OFF, it keeps the older fixed transition behavior while the auto controller only decides the profile switch timing. If a profile hot-switch or direct profile switch still needs a servo handoff fade, that fade starts its own full `mixer_vtol_transition_scale_ramp_time_ms` window instead of using a separate fixed servo delay. +During FW->MC the same MC mixer rules mark which FW servo outputs should reduce fixed-wing correction while motor stabilisation comes back in. +At the same time, target MC motor rules can come in before the switch and use a target MC PID preview, so lift motors are not driven by the active FW/PIFF controller. +These inputs are active only while the smooth auto-transition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. +`INPUT_MIXER_TRANSITION` remains available for tilt/helper servo movement. With `mixer_vtol_transition_dynamic_mixer = ON`, it follows `mixer_vtol_transition_scale_ramp_time_ms`. With it OFF, it keeps the older fixed transition behavior while the auto controller only decides the profile switch timing. If a profile switch or direct switch changes a transition-linked servo output, INAV starts a fresh full `mixer_vtol_transition_scale_ramp_time_ms` movement from the servo's current output instead of using a separate fixed servo delay. Forward motor setup for smooth auto-transition: - Preferred setup: configure the forward motor as a normal positive-throttle motor in the FW mixer profile. - Reserve the same motor index in the MC mixer profile with a placeholder if that motor is not used in MC flight. Use `throttle = -1.000`, `roll = 0`, `pitch = 0`, `yaw = 0` if Configurator removes zero-throttle motor rules. -- During MC->FW, iNAV can ramp that target FW motor rule in before the hot-switch. +- During MC->FW, INAV can bring that target FW motor rule in before the profile switch. - For FW->MC, configure the lift motors as normal positive-throttle motors in the MC mixer profile. If those motor indexes are not used in FW flight, reserve them in the FW mixer profile with placeholders. -- During FW->MC, iNAV can ramp those target MC lift motor rules in before the hot-switch and use target MC stabilisation for their roll/pitch/yaw correction. +- During FW->MC, INAV can bring those target MC lift motor rules in before the profile switch and use target MC stabilisation for their roll/pitch/yaw correction. - The older `-2.0 <= throttle <= -1.05` transition motor rule still works as a legacy/helper path, but it is not the preferred setup for smooth auto-transition. -- If a helper such as `throttle = -1.200` is used before the MC->FW switch, smooth auto-transition fades from that helper output to the real FW mixer output after the hot-switch. -- If the same physical tilt motor is configured with positive throttle in both profiles on the same index, iNAV blends the base throttle between the MC and FW mixer rules instead of adding both throttles together. The current-profile stabilisation fades out while the target-profile stabilisation fades in. +- If a helper such as `throttle = -1.200` is used before the MC->FW switch, smooth auto-transition moves from that helper output to the real FW mixer output after the profile switch. +- If the same physical tilt motor is configured with positive throttle in both profiles on the same index, INAV blends the base throttle between the MC and FW mixer rules instead of adding both throttles together. The current-profile stabilisation is reduced while the target-profile stabilisation is increased. How `mixer_vtol_transition_scale_ramp_time_ms` works: -- Time-based motor/power handover: +- Time-based motor/power and transition-servo movement: - MC->FW: forward motor power ramps from `0 -> 100%` over this time. - - After the MC->FW profile switch, lift motors that are not used by the FW profile fade to idle over this same time instead of stopping immediately. + - After the MC->FW profile switch, lift motors that are not used by the FW profile move to idle over this same time instead of stopping immediately. - FW->MC: forward motor power ramps from `100% -> 0%`, while lift power and MC stabilisation ramp from their configured minimums back to `100%` over this time. - - After the FW->MC profile switch, a forward motor that is not used by the MC profile is kept fading to idle and cannot be reintroduced by FW throttle. + - After the FW->MC profile switch, a forward motor that is not used by the MC profile keeps moving to idle and cannot be reintroduced by FW throttle. + - `INPUT_MIXER_TRANSITION` uses this ramp when `mixer_vtol_transition_dynamic_mixer = ON`; with dynamic mixer OFF it keeps the older fixed transition endpoint behavior. + - If a profile switch or direct switch changes a transition-linked servo output, INAV captures the current servo output and starts a fresh full window from this value. - `= 0` (default): those time-based power changes happen immediately. - This timer does not decide when the transition completes. -- Airspeed-based control handoff: - - in MC->FW, lift power reduction, MC stabilisation fade-out, and FW control fade-in follow airspeed when pitot is usable. - - in FW->MC, FW control fade-out follows airspeed when pitot is usable. +- Airspeed-based control scaling: + - in MC->FW, lift power reduction, MC stabilisation reduction, and FW control increase follow airspeed when pitot is usable. + - in FW->MC, FW control reduction follows airspeed when pitot is usable. - with valid pitot airspeed, they follow transition progress based on airspeed. - without trusted pitot, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). -- In FW->MC, forward motor removal, lift power ramp-in, and MC motor stabilisation ramp-in use the time-based motor ramp so they do not wait for airspeed to fall first. +- In FW->MC, forward motor removal, lift power return, and MC motor stabilisation return use the time-based motor ramp so they do not wait for airspeed to fall first. Example: @@ -217,8 +219,8 @@ Example: Result: - in MC->FW, the forward motor reaches full power in about `1.2s`, - in FW->MC, the forward motor ramps down while lift power and MC stabilisation return in about `1.2s`, -- when pitot is working, control handover still follows airspeed, -- if pitot stops being usable, handover falls back to `mixer_switch_trans_timer`, +- when pitot is working, airspeed-linked control scaling still follows airspeed, +- if pitot stops being usable, airspeed-linked control scaling falls back to `mixer_switch_trans_timer`, - transition completion still uses airspeed when pitot is working, - backup completion time is still `5s` if pitot is not usable. @@ -250,12 +252,12 @@ Setting scope: - `nav_vtol_mission_transition_user_action` - `nav_vtol_mission_transition_min_altitude_cm` -On each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`), the chosen USER flag tells iNAV which flight mode should be active there: +On each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`), the chosen USER flag tells INAV which flight mode should be active there: - selected USER flag = `0` -> target MC / MULTIROTOR profile - selected USER flag = `1` -> target FW / AIRPLANE profile - This is set per waypoint. It is **not** a toggle command. -- If the aircraft is already in the requested mode, iNAV does nothing and continues. +- If the aircraft is already in the requested mode, INAV does nothing and continues. The mission pauses while transition is in progress and resumes after completion. @@ -302,7 +304,7 @@ CLI: Behavior: - Uses pitot airspeed first, with timer fallback only if pitot is not usable. -- Adds smoother forward-motor, lift-motor, and control handover to reduce abrupt transitions. +- Adds smoother forward-motor, lift-motor, and control changes to reduce abrupt transitions. #### Test 3 - Mission-authorized transition (mission integration) @@ -345,8 +347,8 @@ For transition troubleshooting, use: Debug channels: -- `debug[0]` = transition phase (`0=IDLE`, `1=TRANSITION_INITIALIZE`, `2=TRANSITIONING`, `3=POST_SWITCH_FADE`) -- `debug[1]` = active request (`MIXERAT_REQUEST_*` enum value) +- `debug[0]` = transition phase (`0=IDLE`, `1=TRANSITION_INITIALIZE`, `2=TRANSITIONING`, `3=after-switch smoothing`) +- `debug[1]` = active request (`MIXERAT_REQUEST_*` enum value) with transition direction packed in the high byte - `debug[2]` = packed transition flags: - bits 0-1: transition direction (`0=NONE`, `1=TO_FW`, `2=TO_MC`) - bit2: auto-transition controller active @@ -365,10 +367,12 @@ Debug channels: - bit17: failsafe mode active - bit18: manual VTOL auto-transition controller effective after mission gating - bit19: RC `MIXERPROFILE` mode active + - bit20: manual transition session is latched - bits 21-24: active platform type - bits 25-26: active PID type (`pidIndexGetType(PID_ROLL)`) - bits 27-28: target preview mode (`0=none`, `1=manual FW`, `2=target FW PID`, `3=target MC PID`) - - bit29: post-switch fade active + - bit29: after-switch smoothing active + - bit30: legacy manual transition session active - `debug[3]` = progress x1000 (`0..1000`) - `debug[4]` = pusher scale x1000 (`0..1000`) - `debug[5]` = lift scale x1000 (`0..1000`) @@ -376,11 +380,11 @@ Debug channels: - low 16 bits: MC stabilisation scale x1000 (`0..1000`) - high 16 bits: FW control scale x1000 (`0..1000`) - `debug[7]` = packed progress values: - - bits 0-9: handoff progress (`0..1000`) + - bits 0-9: progress used for airspeed-linked scaling (`0..1000`) - bits 10-19: motor ramp progress (`0..1000`) - - bits 20-29: post-switch fade progress (`0..1000`) + - bits 20-29: after-switch smoothing progress (`0..1000`) -Final motor outputs are available in the normal Blackbox motor traces. During post-switch fade, the old lift or pusher motor output should move smoothly toward idle there. +Final motor outputs are available in the normal Blackbox motor traces. During after-switch smoothing, the old lift or pusher motor output should move smoothly toward idle there. ## 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 f73d894c981..3321cced707 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -112,7 +112,7 @@ 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). -- During MC->FW mission transition, iNAV uses a built-in straight run-up target to help the model build speed before switching to fixed-wing. +- During MC->FW mission transition, INAV uses a built-in straight run-up target to help the model build speed before switching to fixed-wing. - VTOL transition completion logic is shared with manual MIXER TRANSITION and uses mixer transition settings: - preferred MC->FW threshold: `vtol_transition_to_fw_min_airspeed_cm_s` - FW->MC threshold: `vtol_transition_to_mc_max_airspeed_cm_s` @@ -142,6 +142,54 @@ Safety and scope: - This path uses authorized automated transition state handling; it does not permit manual mixer profile switching during normal waypoint navigation. - It still depends on valid mixer profile switching infrastructure (two configured mixer profiles and a valid `MIXER PROFILE 2` mode activation condition). +### VTOL MC navigation protection and landing detection + +Targets with more than 512 KB flash can enable extra protection for VTOL aircraft flying in MC mode: + +- `vtol_mc_protection_mode = OFF`: disabled, legacy behavior. +- `vtol_mc_protection_mode = NAV`: protects VTOL MC navigation and altitude-control behavior. +- `vtol_mc_protection_mode = NAV_AND_STABILIZED`: also shapes ANGLE/HORIZON roll, pitch, and yaw commands at higher horizontal speed. + +The protection only activates when the current mixer profile is multicopter-like and another configured mixer profile is fixed-wing. Normal multirotors and fixed-wing mode are not changed. + +In NAV modes, VTOL MC protection adds: + +- throttle reserve before altitude PID anti-windup, controlled by `vtol_mc_thr_reserve_percent`, +- capture/settle when entering position-holding navigation with horizontal speed, +- soft altitude capture while horizontal speed is being bled off, +- a stricter RTH/WP landing settle gate before descent starts, +- a conservative bailout path if attitude becomes excessive while automatic throttle is active. + +The landing settle gate uses `min(nav_wp_radius, 100 cm)` as the capture radius for the landing point. It also requires low horizontal speed, low vertical speed, and safe attitude to be held for the internal settle time. This prevents a large `nav_wp_radius` from starting landing descent after only briefly touching the waypoint radius while still moving. + +ANGLE/HORIZON shaping in `NAV_AND_STABILIZED` only runs when armed, VTOL MC mode is detected, velocity estimate is trusted, and horizontal speed is above the shaping threshold. It continuously scales roll, pitch, and yaw commands as speed increases, preserving command sign and small deadband behavior. + +#### Landing detector sensitivity + +`nav_land_detect_sensitivity` scales the generic landing detector velocity and gyro thresholds. The default `5` is nominal sensitivity. For multirotors, this corresponds to about: + +- `100 cm/s` horizontal speed, +- `100 cm/s` vertical speed, +- `4 deg/s` average pitch/roll gyro rate. + +Higher values relax these thresholds and can make landing detection faster, but also increase false-detect risk. VTOL MC landing detection adds additional safety gates that `nav_land_detect_sensitivity` does not bypass: + +- vertical speed must be near zero when altitude/vertical-speed estimate is available, +- NAV landing must be in the final slow-descent context, +- trusted surface/AGL data, if available, must show near-ground, +- all VTOL MC landing candidates must pass a throttle-probe confirmation before `LANDING_DETECTED`. + +The VTOL MC throttle probe gently reduces lift throttle for a short confirmation window. If the aircraft starts descending, shows unloading acceleration, or trusted AGL drops, the candidate is rejected and the detector waits again. This is intended to avoid false disarm during slow descent or ground-effect bounce, without relying on barometric altitude as AGL. + +`nav_landing_bump_detection = ON` allows G-bump touchdown detection to create a landing candidate. For VTOL MC it is not an immediate disarm shortcut: trusted high AGL blocks it, and accepted candidates still go through the throttle probe. For non-VTOL multirotors it keeps the existing landing detector behavior. + +Automatic disarm still requires `nav_disarm_on_landing = ON`. `nav_auto_disarm_delay` is applied after a landing candidate is detected; in VTOL MC mode the throttle probe must also confirm before the global `LANDING_DETECTED` state is set. + +Debugging: + +- `debug_mode = VTOL_MC_PROTECT` shows protection flags, safe throttle range, protected throttle, speed, attitude, and settle/command-scale progress. +- `debug_mode = LANDING` shows normal landing detector status and candidate state. + `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/docs/Settings.md b/docs/Settings.md index d7d33aeab9e..8ca6a1fcfff 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -795,6 +795,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) | SBUS2 | | | OSD_REFRESH | | | VTOL_TRANSITION | | +| VTOL_MC_PROTECT | | --- @@ -3192,7 +3193,7 @@ This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly ### mixer_automated_switch -If set to on, This mixer_profile will try to switch to another mixer_profile when 1.RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius on mixer_profile is a MULTIROTOR or TRICOPTER platform_type. 2. RTH landing is requested on this mixer_profile is a AIRPLANE platform_type +Allows navigation code to request VTOL mixer profile changes from this profile. In legacy RTH this lets MC/TRICOPTER profiles switch toward FW when returning from far away, and lets AIRPLANE profiles switch toward MC for RTH landing. With the VTOL auto-transition controller, mission waypoints can also request MC/FW transitions through their USER action. Enable this in each profile where NAV is allowed to change the mixer profile. | Default | Min | Max | | --- | --- | --- | @@ -3212,7 +3213,7 @@ If enabled, control_profile_index will follow mixer_profile index. Set to OFF(de ### mixer_switch_trans_timer -Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, the control-handoff side of the transition also falls back to this timing whenever trusted pitot is not usable. +Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, INAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, airspeed-linked power and control changes also fall back to this timer whenever trusted pitot is not usable. | Default | Min | Max | | --- | --- | --- | @@ -3232,7 +3233,7 @@ Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switc ### mixer_vtol_transition_airspeed_timeout_ms -Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. Available only on targets with more than 512 KB flash. +Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, INAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -3242,7 +3243,7 @@ Maximum wait time [ms] for the required pitot airspeed during an airspeed-contro ### mixer_vtol_transition_dynamic_mixer -Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward motor up while lift power and multicopter stabilisation fade down and fixed-wing control fades in. In FW->MC it ramps the forward motor down while target MC lift power and target MC motor stabilisation ramp back in, and fixed-wing control fades down. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash. +Turns on smooth VTOL transition power and control scaling. In MC->FW it smoothly brings the forward motor in while lift power and multicopter stabilisation are reduced and fixed-wing control is increased. In FW->MC it smoothly removes the forward motor while target MC lift power and target MC motor stabilisation come back in, and fixed-wing control is reduced. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -3252,7 +3253,7 @@ Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward ### mixer_vtol_transition_scale_ramp_time_ms -When smooth VTOL transition power changes are ON, this controls the time-based motor/power handover only. In MC->FW it ramps the forward motor from idle to full target power. After the switch to FW, any old lift motors that are no longer used by the FW profile fade to idle over the same time. In FW->MC it ramps the forward motor down to idle and ramps target MC lift power plus target MC motor stabilisation back in. After the switch to MC, any old forward motor that is no longer used by the MC profile is kept fading to idle. `0` applies those time-based power changes immediately. This timer does not decide when the transition completes and it does not control fixed-wing control-surface handoff. Fixed-wing handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash. +Controls time-based smooth VTOL motor, power, and transition-servo movement. In MC->FW it moves the forward motor from idle to the requested power. After the switch to FW, old lift motors that are no longer used by the FW profile are moved to idle over the same time. In FW->MC it moves the forward motor down to idle while target MC lift power plus target MC motor stabilisation come back in. `INPUT_MIXER_TRANSITION` also uses this time when `mixer_vtol_transition_dynamic_mixer` is ON. If a profile switch or direct switch changes a transition-linked servo output, INAV starts a fresh smooth movement from the servo's current output using this full time value. `0` applies these time-based changes immediately. This timer does not decide when transition completes; completion still uses trusted pitot airspeed or `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -4022,7 +4023,7 @@ Deadband for heading trajectory PID controller. When heading error is below the ### nav_land_detect_sensitivity -Changes sensitivity of landing detection. Higher values increase speed of detection but also increase risk of false detection. Default value should work in most cases. +Scales the generic landing detector velocity and gyro thresholds. `5` is nominal sensitivity: for MC this is about 100 cm/s horizontal and vertical speed plus 4 deg/s average pitch/roll gyro rate. Higher values relax these thresholds and can detect landing sooner, but increase false-detect risk. VTOL MC landing detection also uses additional hard vertical-speed and throttle-probe safety gates that this setting does not bypass. | Default | Min | Max | | --- | --- | --- | @@ -4072,7 +4073,7 @@ Defines at what altitude the descent velocity should start to be `nav_land_minal ### nav_landing_bump_detection -Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS). +Allows landing detection to use a touchdown G-bump candidate when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS). For VTOL MC mode the G-bump is not an immediate disarm shortcut: trusted high AGL blocks it, and accepted candidates must still pass the VTOL MC throttle-probe confirmation. | Default | Min | Max | | --- | --- | --- | @@ -4689,7 +4690,7 @@ Do not start a mission-requested VTOL transition below this altitude [cm]. Set t ### nav_vtol_mission_transition_user_action -Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles, a working `MIXER PROFILE 2` mode setup, and a target with more than 512 KB flash. +Chooses which waypoint USER flag (`USER1`..`USER4`) tells INAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles, a working `MIXER PROFILE 2` mode setup, and a target with more than 512 KB flash. | Allowed Values | | | --- | --- | @@ -4703,7 +4704,7 @@ Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode ### nav_vtol_transition_fail_action_fw_to_mc -What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. Available only on targets with more than 512 KB flash. +What INAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. Available only on targets with more than 512 KB flash. | Allowed Values | | | --- | --- | @@ -4717,7 +4718,7 @@ What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near ### nav_vtol_transition_fail_action_mc_to_fw -What iNAV should do if MC->FW transition still fails after the final attempt. Available only on targets with more than 512 KB flash. +What INAV should do if MC->FW transition still fails after the final attempt. Available only on targets with more than 512 KB flash. | Allowed Values | | | --- | --- | @@ -4730,7 +4731,7 @@ What iNAV should do if MC->FW transition still fails after the final attempt. Av ### nav_vtol_transition_retry_on_airspeed_timeout -If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. Available only on targets with more than 512 KB flash. +If ON, INAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7100,7 +7101,7 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, ### vtol_fw_to_mc_auto_switch_airspeed_cm_s -Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. Available only on targets with more than 512 KB flash. +Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, INAV automatically starts FW->MC. After the switch to MC, INAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7108,9 +7109,31 @@ Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to th --- +### vtol_mc_protection_mode + +Enables extra protection for VTOL aircraft while the active mixer profile is multicopter-like and another mixer profile is fixed-wing. OFF keeps legacy behavior. NAV protects altitude/NAV throttle authority, NAV capture/settle, landing settle, VTOL MC landing confirmation, and conservative bailout. NAV_AND_STABILIZED also applies speed-based roll/pitch/yaw command shaping in ANGLE/HORIZON when armed, VTOL MC mode is detected, velocity is trusted, and horizontal speed is high enough. Available only on targets with more than 512 KB flash. + +| Allowed Values | | +| --- | --- | +| OFF | Default | +| NAV | | +| NAV_AND_STABILIZED | | + +--- + +### vtol_mc_thr_reserve_percent + +Throttle range percentage reserved on both low and high side for VTOL MC attitude authority while navigation/altitude control owns throttle. The reserve is applied before altitude PID anti-windup bounds and is automatically shrunk if needed to keep hover throttle inside the usable range. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 0 | 40 | + +--- + ### vtol_transition_fw_authority_min_percent -Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength as the handoff progresses. In FW->MC, it fades down from full strength to this value as the handoff progresses. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength as the transition progresses. In FW->MC, it is reduced from full strength to this value as the transition progresses. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7120,7 +7143,7 @@ Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, f ### vtol_transition_lift_min_percent -Lowest lift motor power used during transition, in percent. In MC->FW, lift power fades down to this value as the handoff progresses. In FW->MC, lift power starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest lift motor power used during transition, in percent. In MC->FW, lift power is reduced to this value as the transition progresses. In FW->MC, lift power starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7130,7 +7153,7 @@ Lowest lift motor power used during transition, in percent. In MC->FW, lift powe ### vtol_transition_mc_authority_min_percent -Lowest multicopter stabilisation used during transition, in percent. In MC->FW, active MC motor stabilisation fades down to this value as the handoff progresses. In FW->MC, target MC motor stabilisation starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. +Lowest multicopter stabilisation used during transition, in percent. In MC->FW, active MC motor stabilisation is reduced to this value as the transition progresses. In FW->MC, target MC motor stabilisation starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7140,7 +7163,7 @@ Lowest multicopter stabilisation used during transition, in percent. In MC->FW, ### vtol_transition_to_fw_min_airspeed_cm_s -Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash. +Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7150,7 +7173,7 @@ Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered comp ### vtol_transition_to_mc_max_airspeed_cm_s -When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash. +When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -7311,3 +7334,4 @@ Defines rotation rate on YAW axis that UAV will try to archive on max. stick def | 20 | 1 | 180 | --- + diff --git a/docs/VTOL.md b/docs/VTOL.md index edb4655a2af..f4f750d5659 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -60,7 +60,7 @@ set motor_pwm_protocol = DSHOT300 #Try dshot first and see if it works set airmode_type = STICK_CENTER_ONCE -set nav_disarm_on_landing = OFF #band-aid for false landing detection in NAV landing of multi-copter +set nav_disarm_on_landing = OFF #Recommended for first VTOL tests; enable only after validating landing detection on your airframe set nav_rth_allow_landing = FS_ONLY set nav_wp_max_safe_distance = 500 set nav_fw_control_smoothness = 2 @@ -269,367 +269,753 @@ No additional settings needed, 45 deg offset will be added to target pitch angle ### With aforementioned settings, your model should be able to enter fixed-wing profile without stalling. -# Automated Switching (RTH) (Optional): -### This is one of the least tested features. This feature is primarily designed for Return to Home (RTH) in the event of a failsafe. -When configured correctly, the model will use the Fixed-Wing (FW) mode to efficiently return home and then transition to Multi-Copter (MC) mode for easier landing. +# Smooth VTOL Auto-Transition Setup -To enable this feature, type following command in cli +This section describes the new VTOL auto-transition features as a practical setup path. It is written as a sequence of small steps: + +1. Manual switch auto transition +2. Manual switch auto transition with dynamic scaling +3. Automated mission transition +4. Adding VTOL MC stabilisation protection +5. Landing detection setup + +The examples assume: + +- mixer profile 1 is the fixed-wing profile (`FW`, usually `platform_type = AIRPLANE`) +- mixer profile 2 is the multicopter profile (`MC`, for example `MULTIROTOR` or `TRICOPTER`) +- `MIXER PROFILE 2` mode selects the MC profile when you fly manually +- the model has been bench-tested without propellers before flight + +The smooth auto-transition controller is available only on targets with more than 512 KB flash. Smaller targets keep the older VTOL transition behavior and do not include these new settings. + +## 1. Manual switch auto transition + +Manual switch auto transition means that `MIXER TRANSITION` starts one complete transition. You no longer need to manually time the exact profile switch point. INAV starts the transition, waits for the configured speed or timer condition, then changes to the target mixer profile. + +This does not remove the older behavior. If `mixer_vtol_manualswitch_autotransition_controller = OFF`, manual transition stays as close as possible to the older INAV behavior. + +### Recommended first setup + +Configure both mixer profiles. This makes MC -> FW and FW -> MC behave consistently. + +Per-mixer-profile settings: -1. In your MC mode mixer profile (e.g., mixer_profile 2), set `mixer_automated_switch` to `ON`. leave it to `OFF` if burning remaining battery capacity on the way home is acceptable. ``` -mixer_profile 2or1 -set mixer_automated_switch= ON +mixer_profile 1 +set mixer_vtol_manualswitch_autotransition_controller = ON +set mixer_vtol_transition_dynamic_mixer = OFF +set mixer_switch_trans_timer = 50 +set mixer_vtol_transition_airspeed_timeout_ms = 0 +set mixer_vtol_transition_scale_ramp_time_ms = 0 + +mixer_profile 2 +set mixer_vtol_manualswitch_autotransition_controller = ON +set mixer_vtol_transition_dynamic_mixer = OFF +set mixer_switch_trans_timer = 50 +set mixer_vtol_transition_airspeed_timeout_ms = 0 +set mixer_vtol_transition_scale_ramp_time_ms = 0 ``` -2. Set `mixer_switch_trans_timer` ds in cli in the MC mode mixer profile to specify the time required for your model to gain sufficient airspeed before transitioning to FW mode. +Global settings: + ``` -mixer_profile 2or1 -set mixer_switch_trans_timer = 30 # 3s, 3000ms +set vtol_transition_to_fw_min_airspeed_cm_s = 0 +set vtol_transition_to_mc_max_airspeed_cm_s = 0 +set nav_vtol_mission_transition_user_action = OFF +save ``` -3. In your FW mode mixer profile (e.g., mixer_profile 1), also set `mixer_automated_switch` to `ON`. leave it to `OFF` if automated landing in fixed-wing is acceptable. + +What this setup does: + +- `MIXER TRANSITION` starts one transition each time it moves from OFF to ON. +- Leaving `MIXER TRANSITION` ON does not restart the transition repeatedly. +- To request another transition, turn `MIXER TRANSITION` OFF, then ON again. +- If you turn `MIXER TRANSITION` OFF before the profile switch happens, INAV cancels that transition request. +- `mixer_vtol_transition_dynamic_mixer = OFF` keeps transition motor/servo behavior close to the older setup while the new controller only manages the timing of the profile switch. +- `mixer_switch_trans_timer = 50` means `5.0s`, because this setting is in deciseconds. + +Value direction notes: + +- Lower `mixer_switch_trans_timer`: timer-based transitions complete sooner, but may switch before the aircraft has enough speed. +- Higher `mixer_switch_trans_timer`: gives more time to accelerate or slow down, but keeps the aircraft in transition longer when pitot is not used. +- `mixer_vtol_transition_airspeed_timeout_ms = 0`: disables airspeed timeout aborts. Higher values wait longer before aborting an airspeed-controlled attempt; lower values abort sooner. +- `mixer_vtol_transition_scale_ramp_time_ms = 0`: no extra smooth motor/servo movement in this legacy-compatible baseline. Higher values are useful only when dynamic scaling is ON. +- `vtol_transition_to_fw_min_airspeed_cm_s = 0` and `vtol_transition_to_mc_max_airspeed_cm_s = 0`: use timer completion. Higher non-zero values enable pitot-based completion and make the speed condition stricter. +- `nav_vtol_mission_transition_user_action = OFF`: mission transition is disabled. Set it to `USER1`..`USER4` only after manual transition is validated. + +Typical 3-position switch layout: + +- Position 1: FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) +- Position 2: transition request (`MIXER PROFILE 2` ON, `MIXER TRANSITION` ON) +- Position 3: MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) + +Workflow: + +- Start in MC. +- Move to the transition position. +- INAV starts MC -> FW and changes profile after the configured condition is met. +- Move the switch to the FW position after the transition completes. +- Reverse the process for FW -> MC. + +In the standard layout above, the transition position deliberately has both `MIXER PROFILE 2` and `MIXER TRANSITION` ON. While the auto-transition controller is enabled, the controller owns the profile switching until `MIXER TRANSITION` is turned OFF again. + +### Timer, pitot, and timeout examples + +`mixer_switch_trans_timer` is the backup completion timer. It is used when pitot airspeed is not configured, not trusted, or not available. + +Examples: + +- `mixer_switch_trans_timer = 30`: transition completes by timer after `3.0s` if pitot is not used. +- `mixer_switch_trans_timer = 50`: transition completes by timer after `5.0s` if pitot is not used. +- `vtol_transition_to_fw_min_airspeed_cm_s = 1300`: MC -> FW waits for `13 m/s` pitot airspeed when pitot is trusted. +- `vtol_transition_to_mc_max_airspeed_cm_s = 850`: FW -> MC waits until pitot airspeed falls to `8.5 m/s` or lower when pitot is trusted. +- `mixer_vtol_transition_airspeed_timeout_ms = 6500`: if pitot remains trusted but the requested airspeed is not reached within `6.5s`, that airspeed-controlled transition attempt is aborted. + +Important pitot behavior: + +- If pitot is trusted and a non-zero airspeed threshold is configured, INAV prefers airspeed for transition completion. +- The configured airspeed must be reached before `mixer_vtol_transition_airspeed_timeout_ms` expires. For example, with `vtol_transition_to_fw_min_airspeed_cm_s = 1300` and `mixer_vtol_transition_airspeed_timeout_ms = 6500`, MC -> FW must reach `13 m/s` within `6.5s`. +- If that timeout expires during a manual transition, the transition attempt is aborted and INAV does not force the target profile switch from that timeout. +- If that timeout expires during a mission transition, mission retry/failure handling is used: retry can run if `nav_vtol_transition_retry_on_airspeed_timeout = ON`; otherwise the configured mission fail action is used. +- If pitot becomes unavailable during the transition, INAV falls back to `mixer_switch_trans_timer`. +- Ground speed is not used to decide transition completion. +- `mixer_vtol_transition_airspeed_timeout_ms` does not complete a transition. It only stops an airspeed-controlled attempt that is taking too long. + +Optional low-speed protection: + ``` -mixer_profile 1or2 -set mixer_automated_switch = ON +set vtol_fw_to_mc_auto_switch_airspeed_cm_s = 750 +``` + +With this set, fixed-wing flight automatically starts FW -> MC when trusted pitot airspeed drops to `7.5 m/s` or lower. After this protection switches to MC, INAV stays in MC until you deliberately command another manual profile change. Set it to `0` to disable this protection. + +## 2. Manual switch auto transition with dynamic scaling + +Dynamic scaling is the optional smooth part of the new transition system. It lets INAV change motor power and stabilisation strength gradually instead of making one large step at the profile switch. + +Enable it in both VTOL profiles: + +Per-mixer-profile settings: + +``` +mixer_profile 1 +set mixer_vtol_manualswitch_autotransition_controller = ON +set mixer_vtol_transition_dynamic_mixer = ON +set mixer_switch_trans_timer = 50 +set mixer_vtol_transition_airspeed_timeout_ms = 6500 +set mixer_vtol_transition_scale_ramp_time_ms = 1200 + +mixer_profile 2 +set mixer_vtol_manualswitch_autotransition_controller = ON +set mixer_vtol_transition_dynamic_mixer = ON +set mixer_switch_trans_timer = 50 +set mixer_vtol_transition_airspeed_timeout_ms = 6500 +set mixer_vtol_transition_scale_ramp_time_ms = 1200 +``` + +Global settings: + ``` -4. Save your settings. type `save` in cli. +set vtol_transition_to_fw_min_airspeed_cm_s = 1300 +set vtol_transition_to_mc_max_airspeed_cm_s = 850 +set vtol_transition_lift_min_percent = 30 +set vtol_transition_mc_authority_min_percent = 20 +set vtol_transition_fw_authority_min_percent = 20 +save +``` + +What dynamic scaling changes: + +- MC -> FW can smoothly bring in the forward motor before the profile switch. +- MC -> FW can reduce lift motor power and MC motor stabilisation while fixed-wing control is increased. +- FW -> MC can remove the forward motor while lift motors and MC motor stabilisation come back. +- FW -> MC can reduce fixed-wing control as MC control comes back. +- Transition-linked servos continue from their current output if the profile switch would otherwise cause a step. + +What it does not change: + +- It does not decide by itself when the transition is complete. +- Completion still uses trusted pitot airspeed first, or `mixer_switch_trans_timer` when pitot is not used. +- With `mixer_vtol_transition_dynamic_mixer = OFF`, the old transition input behavior is preserved and the auto-controller only manages the profile switch timing. + +Value direction notes: + +- Lower `mixer_vtol_transition_scale_ramp_time_ms`: pusher, lift return, and tilt servo movement happen faster. Too low can still look abrupt. +- Higher `mixer_vtol_transition_scale_ramp_time_ms`: movement is gentler, but the aircraft spends longer with partial pusher/lift/servo authority. +- Lower `vtol_transition_lift_min_percent`: lift motors reduce more during MC -> FW. This can reduce drag/power use but gives less lift reserve. +- Higher `vtol_transition_lift_min_percent`: more lift is kept through transition. `100` keeps full lift power. +- Lower `vtol_transition_mc_authority_min_percent`: MC motor stabilisation is reduced more during MC -> FW. Use carefully on large VTOLs. +- Higher `vtol_transition_mc_authority_min_percent`: more MC stabilisation stays available. `100` keeps full MC stabilisation. +- Lower `vtol_transition_fw_authority_min_percent`: fixed-wing control starts more gently. Higher values bring fixed-wing control in more strongly from the start. +- Lower airspeed thresholds complete sooner; higher thresholds wait for more airspeed before switching profile. + +### `mixer_vtol_transition_scale_ramp_time_ms` + +This setting controls the time-based smooth movement for: -If you set `mixer_automated_switch` to `OFF` for all mixer profiles (the default setting), the model will not perform automated transitions. You can always enable navigation modes after performing a manual transition. - -## Unified VTOL Transition Controller (Manual + Mission) - -This feature is available only on targets with more than 512 KB flash. -In standard INAV builds those targets are compiled with `USE_AUTO_TRANSITION`. -Targets with 512 KB flash keep the older VTOL mixer transition behavior and do not include the smooth auto-transition settings. - -INAV now uses one internal VTOL transition controller for both: -- manual `MIXER TRANSITION` requests, and -- mission-authorized VTOL transitions. - -This keeps one safety boundary for profile changes and avoids separate transition implementations. - -### Behavior summary - -- iNAV always tracks transition progress internally. -- If valid pitot airspeed is available, airspeed is the main way iNAV decides when transition is complete. -- If pitot is not available, iNAV falls back to a timer. -- Ground speed is not used for transition completion. -- Mission VTOL transition uses the same controller and does not directly drive the motors by itself. -- During normal waypoint navigation, manual `MIXER PROFILE` and `MIXER TRANSITION` switching is still blocked. -- `MIXER PROFILE 2` is still a direct manual profile switch when you are flying manually. -- Smooth automatic transition is started by `MIXER TRANSITION` when the manual auto-controller is ON, or by a mission transition request. - -### Manual transition semantics - -This does not remove the older manual behavior. The older behavior is still available if you want it. - -With `mixer_vtol_manualswitch_autotransition_controller = ON`: -- Turn this ON in both mixer profiles if you want the same behavior in both directions. -- Each time `MIXER TRANSITION` moves from OFF to ON, iNAV starts one transition. -- After it starts, the transition keeps running until the speed target or timer target is reached. -- Leaving the switch ON does not keep restarting the transition. -- To start another transition, turn the switch OFF and then ON again. -- If you turn the switch OFF before the profile change happens, that transition request is cancelled. -- Optional extra protection: set `vtol_fw_to_mc_auto_switch_airspeed_cm_s > 0` if you want FW->MC to start automatically when pitot airspeed becomes too low. After that switch, iNAV stays in MC until you deliberately command another manual profile change. - -With `mixer_vtol_manualswitch_autotransition_controller = OFF`: -- the older manual behavior is preserved. - -Typical 3-position switch workflow: -- Position 1: FW -- Position 2: Transition request -- Position 3: MC - -Operational example: -- fly in MC (pos3) -> move to Transition (pos2) to start automatic MC->FW transition -> after completion move to FW (pos1) -- reverse the order for FW->MC - -Important RC mapping constraint: -- One supported mapping is: - - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) - - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) - - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) -- Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. -- Another supported mapping is the overlap version: while both `MIXER PROFILE 2` and `MIXER TRANSITION` are ON, the transition controller owns the switching until `MIXER TRANSITION` turns OFF again. - -### Mission-authorized transition semantics - -Mission transition is configured with `nav_vtol_mission_transition_user_action`. - -- `OFF`: feature disabled. -- `USER1`..`USER4`: the selected USER flag becomes the flight-mode selector on navigable waypoints. -- selected flag `0` -> target MC profile -- selected flag `1` -> target FW profile -- when this feature is ON, every navigable waypoint should intentionally have that USER flag either clear or set -- Mission progression pauses during transition and resumes only after completion. -- If the aircraft is already in the requested mode, iNAV does nothing and continues. - -For MC -> FW mission transition: -- guidance uses a straight acceleration run, -- normal waypoint advancement is paused during transition. - -### Airspeed-first completion logic - -MC -> FW: -- `vtol_transition_to_fw_min_airspeed_cm_s` is the target airspeed. -- If pitot stops being usable, or if this is `0`, MC->FW uses `mixer_switch_trans_timer` instead. - -FW -> MC: -- `vtol_transition_to_mc_max_airspeed_cm_s` is the airspeed that must be reached or lower. -- If pitot stops being usable, or if this is `0`, FW->MC uses `mixer_switch_trans_timer` instead. - -Timeout: -- `mixer_switch_trans_timer` is the original VTOL transition timer. It is still the backup completion timer when trusted pitot airspeed is not being used. -- `mixer_vtol_transition_airspeed_timeout_ms` is only a maximum wait time for the required airspeed while pitot is still usable. It does not complete the transition by itself; it aborts that airspeed-controlled attempt. -- If pitot stops being usable, iNAV stops using the airspeed timeout and falls back to `mixer_switch_trans_timer`. -- For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). - -### Smooth power changes during transition - -When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: -- forward motor power, -- lift motor power, -- multicopter stabilisation strength, -- fixed-wing control strength. - -When `mixer_vtol_transition_dynamic_mixer = OFF`, the older static behavior is preserved. -When it is ON, you can configure `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules in the MC mixer profile. -During MC->FW they give those servos a preview of the fixed-wing stabilisation that will take over after the hot-switch. -This preview uses the target fixed-wing PID bank, rates, angle limits, heading-hold limits, and turn-assist gains, but it still follows the current transition stick shaping until the actual profile switch. -During FW->MC the same MC mixer rules mark which FW servo outputs should fade down as fixed-wing authority is reduced and motor stabilisation comes back in. -At the same time, target MC motor rules can fade in before the switch and use a target MC PID preview, so lift motors are not driven by the active FW/PIFF controller. -These inputs are active only while the smooth autotransition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. -`INPUT_MIXER_TRANSITION` remains available for tilt/helper servo movement. With `mixer_vtol_transition_dynamic_mixer = ON`, it follows `mixer_vtol_transition_scale_ramp_time_ms`. With it OFF, it keeps the older fixed transition behavior while the auto controller only decides the profile switch timing. If a profile hot-switch or direct profile switch still needs a servo handoff fade, that fade starts its own full `mixer_vtol_transition_scale_ramp_time_ms` window instead of using a separate fixed servo delay. - -Forward motor setup for smooth auto-transition: - -- Preferred setup: configure the forward motor as a normal positive-throttle motor in the FW mixer profile. -- Reserve the same motor index in the MC mixer profile with a placeholder if that motor is not used in MC flight. Use `throttle = -1.000`, `roll = 0`, `pitch = 0`, `yaw = 0` if Configurator removes zero-throttle motor rules. -- During MC->FW, iNAV can ramp that target FW motor rule in before the hot-switch. -- For FW->MC, configure the lift motors as normal positive-throttle motors in the MC mixer profile. If those motor indexes are not used in FW flight, reserve them in the FW mixer profile with placeholders. -- During FW->MC, iNAV can ramp those target MC lift motor rules in before the hot-switch and use target MC stabilisation for their roll/pitch/yaw correction. -- The older `-2.0 <= throttle <= -1.05` transition motor rule still works as a legacy/helper path, but it is not the preferred setup for smooth auto-transition. -- If a helper such as `throttle = -1.200` is used before the MC->FW switch, smooth auto-transition fades from that helper output to the real FW mixer output after the hot-switch. -- If the same physical tilt motor is configured with positive throttle in both profiles on the same index, iNAV blends the base throttle between the MC and FW mixer rules instead of adding both throttles together. The current-profile stabilisation fades out while the target-profile stabilisation fades in. - -`mixer_vtol_transition_scale_ramp_time_ms` controls the time-based motor/power handover when this feature is ON. -It does not decide when the transition completes. - -How `mixer_vtol_transition_scale_ramp_time_ms` works: -- Time-based motor/power handover: - - MC->FW: forward motor power ramps from `0 -> 100%` over this time. - - After the MC->FW profile switch, lift motors that are not used by the FW profile fade to idle over this same time instead of stopping immediately. - - FW->MC: forward motor power ramps from `100% -> 0%`, while lift power and MC stabilisation ramp from their configured minimums back to `100%` over this time. - - After the FW->MC profile switch, a forward motor that is not used by the MC profile is kept fading to idle and cannot be reintroduced by FW throttle. - - `= 0` (default): those time-based power changes happen immediately. -- Airspeed-based control handoff: - - in MC->FW, lift power reduction, MC stabilisation fade-out, and FW control fade-in follow airspeed when pitot is usable. - - in FW->MC, FW control fade-out follows airspeed when pitot is usable. - - if pitot is not usable, those handoff changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). -- In FW->MC, forward motor removal, lift power ramp-in, and MC motor stabilisation ramp-in use the time-based motor ramp so they do not wait for airspeed to fall first. +- MC -> FW forward motor power increase +- FW -> MC forward motor power removal +- FW -> MC lift motor and MC motor stabilisation return +- `INPUT_MIXER_TRANSITION` movement when `mixer_vtol_transition_dynamic_mixer = ON` +- servo output continuation after a direct profile switch or transition abort changes the active mixer output Example: -- `mixer_switch_trans_timer = 50` (5s fallback completion timer) + - `mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `mixer_switch_trans_timer = 50` Result: -- in MC->FW, the forward motor reaches full power in about `1.2s`, -- in FW->MC, the forward motor ramps down while lift power and MC stabilisation return in about `1.2s`, -- when pitot is working, control handover still follows airspeed, -- if pitot is not usable, handover falls back to `mixer_switch_trans_timer`, -- transition completion still uses airspeed when pitot is working, -- backup completion time is still `5s` if pitot is not usable. - -### Example test presets (VTOL ~1.0m wingspan, ~1720g AUW) - -These are example starting points for initial testing. They are not universal values; tune after bench tests and short flight tests. - -#### Test 1 - Legacy-compatible baseline (manual transition check) - -Goal: -- Verify that the new controller does not change legacy behavior when smooth power changes are disabled. -- Good first test after flashing. - -CLI: -- `set mixer_vtol_manualswitch_autotransition_controller = ON` -- `set mixer_vtol_transition_dynamic_mixer = OFF` -- `set mixer_switch_trans_timer = 45` -- `set vtol_transition_to_fw_min_airspeed_cm_s = 0` -- `set vtol_transition_to_mc_max_airspeed_cm_s = 900` -- `set mixer_vtol_transition_airspeed_timeout_ms = 0` -- `set mixer_vtol_transition_scale_ramp_time_ms = 0` -- `set nav_vtol_mission_transition_user_action = OFF` - -What this does: -- Keeps transition mixing behavior close to legacy mode. -- Uses timer-driven completion when no trusted pitot threshold is configured. -- Uses conservative FW->MC completion threshold. -- Disables mission-authorized transition while validating manual behavior. - -#### Test 2 - Airspeed-first + smooth power changes (manual transition tuning) - -Goal: -- Enable the full new behavior: airspeed-first completion and smooth forward-motor and control handover. - -CLI: -- `set mixer_vtol_manualswitch_autotransition_controller = ON` -- `set mixer_vtol_transition_dynamic_mixer = ON` -- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` -- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` -- `set mixer_switch_trans_timer = 50` -- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` -- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` -- `set vtol_transition_lift_min_percent = 30` -- `set vtol_transition_mc_authority_min_percent = 20` -- `set vtol_transition_fw_authority_min_percent = 20` -- `set nav_vtol_mission_transition_user_action = OFF` - -What this does: -- MC->FW completes primarily on pitot airspeed (1300 cm/s), with timer fallback only if pitot is unavailable/unhealthy. -- FW->MC completes when airspeed drops to 850 cm/s. -- In MC->FW, the forward motor ramps to full power in `1.2s` while lift power and control handover still follow airspeed progress. -- The pusher ramp is quick enough (1.2 s) to reduce step torque while still allowing strong acceleration. -- Timeout abort protects against staying too long in airspeed-controlled transition without reaching threshold. - -#### Test 3 - Mission-authorized transition (end-to-end mission flow) - -Goal: -- Validate mission User Action integration and pause/resume behavior. - -CLI: -- `set mixer_vtol_manualswitch_autotransition_controller = ON` -- `set mixer_vtol_transition_dynamic_mixer = ON` -- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` -- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` -- `set mixer_switch_trans_timer = 50` -- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` -- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` -- `set vtol_transition_lift_min_percent = 30` -- `set vtol_transition_mc_authority_min_percent = 20` -- `set vtol_transition_fw_authority_min_percent = 20` -- `set nav_vtol_mission_transition_user_action = USER1` -- `set nav_vtol_mission_transition_min_altitude_cm = 1200` - -What this does: -- Uses USER1 as the absolute per-waypoint target selector: - - USER1 bit clear -> target MC - - USER1 bit set -> target FW -- Pauses mission progression during transition and resumes after completion. -- Uses a straight MC->FW acceleration segment (no loiter) before the switch to fixed-wing. -- Adds a minimum altitude gate (12 m) before mission transition starts. - -### Detailed effect of the three percentage settings - -These three settings are active only when `mixer_vtol_transition_dynamic_mixer = ON`. - -1. `vtol_transition_lift_min_percent` -- Sets the lowest lift motor power used during transition. -- MC -> FW: lift power goes from `100%` at start down to `lift_min_percent`. -- FW -> MC: lift power goes from `lift_min_percent` at start up to `100%`. - -Example (`vtol_transition_lift_min_percent = 20`): -- MC -> FW at 50% progress: lift power is about `60%`. -- FW -> MC at 50% progress: lift power is about `60%`. - -2. `vtol_transition_mc_authority_min_percent` -- Sets the lowest multicopter stabilisation used during transition. -- MC -> FW: active MC motor stabilisation goes from `100%` at start down to `mc_authority_min_percent`. -- FW -> MC: target MC motor stabilisation goes from `mc_authority_min_percent` at start up to `100%`. -- During FW -> MC, this target MC stabilisation comes from the MC mixer profile and target MC PID preview, not from the active FW/PIFF controller. - -Example (`vtol_transition_mc_authority_min_percent = 30`): -- MC -> FW at 50% progress: MC stabilisation is about `65%`. -- FW -> MC at 50% progress: MC stabilisation is about `65%`. - -3. `vtol_transition_fw_authority_min_percent` -- Sets the lowest fixed-wing control used during transition. -- MC -> FW: fixed-wing control goes from `fw_authority_min_percent` at start up to `100%`. -- FW -> MC: fixed-wing control goes from `100%` at start down to `fw_authority_min_percent`. -- During MC -> FW, this same setting also scales `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules configured in the MC mixer profile. -- During FW -> MC, the same setting scales down the matching FW servo stabilisation on the outputs marked by those MC mixer rules. - -Example (`vtol_transition_fw_authority_min_percent = 25`): -- MC -> FW at 50% progress: fixed-wing control is about `62.5%`. -- FW -> MC at 50% progress: fixed-wing control is about `62.5%`. - -Practical note: -- `vtol_transition_fw_authority_min_percent = 100` keeps full fixed-wing control through the whole transition. -- Lower values bring fixed-wing control in and out more gently. - -## Setting Scope (Important) -The new VTOL settings are split into two groups: +- The forward motor moves from idle to requested power over `1.2s` during MC -> FW. +- A tilt servo using `INPUT_MIXER_TRANSITION` moves over `1.2s` while dynamic scaling is ON. +- If the profile switch changes the final servo output, the servo continues from its current output and moves toward the new output over a fresh `1.2s`. +- If pitot is not used, the profile switch still happens after `5.0s`. +- If pitot is trusted, the profile switch waits for the configured pitot threshold instead of the `5.0s` timer. + +This separation is intentional. Tilt servos should not wait for airspeed to build before they start moving; otherwise the aircraft may need tilt to gain speed but also need speed before it is allowed to tilt. The tilt servo source uses `mixer_vtol_transition_scale_ramp_time_ms` when dynamic scaling is ON. + +### Percentage settings + +These settings are active only when `mixer_vtol_transition_dynamic_mixer = ON`. + +`vtol_transition_lift_min_percent` + +- Current purpose: sets the lowest lift motor power used during transition. +- MC -> FW: lift power is reduced toward this value. +- FW -> MC: lift power starts from this value and returns to full power. +- `100` keeps full lift power for the whole transition. + +Example: + +- `vtol_transition_lift_min_percent = 30` +- At halfway through the lift-power change, lift power is about `65%`. +- At the lowest point, lift power is `30%`, not zero. + +`vtol_transition_mc_authority_min_percent` + +- Current purpose: sets the lowest MC motor stabilisation strength during transition. +- MC -> FW: active MC motor stabilisation is reduced toward this value. +- FW -> MC: target MC motor stabilisation starts from this value and returns to full strength. +- During FW -> MC, this target MC stabilisation comes from the MC mixer profile and target MC PID preview, not from the active FW controller. + +Example: + +- `vtol_transition_mc_authority_min_percent = 20` +- MC motor stabilisation never goes below `20%` during the transition. + +`vtol_transition_fw_authority_min_percent` + +- Current purpose: sets the lowest fixed-wing stabilisation strength during transition. +- MC -> FW: fixed-wing control starts from this value and increases to full strength. +- FW -> MC: fixed-wing control is reduced toward this value. +- When `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules are configured in the MC profile, this setting also controls how strongly those target fixed-wing servo corrections are applied during MC -> FW. + +Example: + +- `vtol_transition_fw_authority_min_percent = 20` +- Fixed-wing control starts gently, then increases as the transition progresses. +- `100` means fixed-wing control is full strength for the whole transition. + +### Pusher configuration example + +For a pusher VTOL, the preferred smooth setup is: + +- In the FW profile, configure the pusher as a normal positive-throttle motor. +- In the MC profile, reserve the same motor index with a placeholder rule if that motor is not used in MC flight. +- Use `throttle = -1.000`, `roll = 0`, `pitch = 0`, `yaw = 0` for the placeholder if Configurator removes zero-throttle motor rules. + +Example shape: + +``` +# MC profile: reserve motor 5 as a placeholder +mixer_profile 2 +mmix 4 -1.000 0.000 0.000 0.000 + +# FW profile: motor 5 is the real forward motor +mixer_profile 1 +mmix 4 1.000 0.000 0.000 0.000 +``` + +What happens with dynamic scaling ON: + +- MC -> FW: motor 5 starts from idle and smoothly reaches the requested FW throttle over `mixer_vtol_transition_scale_ramp_time_ms`. +- FW -> MC: motor 5 smoothly moves back to idle over `mixer_vtol_transition_scale_ramp_time_ms`. +- If motor 5 is not used by the destination profile after the switch, INAV keeps moving it toward idle instead of stopping it in one step. + +The older helper rule still works: + +``` +mmix 4 -1.200 0.000 0.000 0.000 +``` + +That older style spins the pusher at a fixed helper power only while `MIXER TRANSITION` is active. It can still be useful for legacy setups, but it is not the recommended setup for the smooth auto-transition controller. + +### Tilt servo configuration example + +Tilt servos commonly use: + +- `INPUT_MAX` (`29`) as a fixed offset +- `INPUT_MIXER_TRANSITION` (`38`) as the transition movement +- `INPUT_STABILIZED_YAW` (`2`) for tricopter/tiltmotor yaw correction + +Example based on a tricopter/tiltmotor style setup: + +``` +mixer_profile 2 +set platform_type = TRICOPTER + +smix reset +smix 0 4 2 -50 0 -1 # yaw correction on servo 4 +smix 1 5 2 -50 0 -1 # yaw correction on servo 5 +smix 2 4 29 87 0 -1 # fixed tilt offset on servo 4 +smix 3 5 29 -87 0 -1 # fixed tilt offset on servo 5 +smix 4 4 38 -45 0 -1 # transition tilt movement on servo 4 +smix 5 5 38 45 0 -1 # transition tilt movement on servo 5 +``` + +Behavior: + +- `INPUT_MAX` stays constant. +- With dynamic scaling OFF, `INPUT_MIXER_TRANSITION` keeps the older fixed transition value while transition mode is active. +- With dynamic scaling ON, `INPUT_MIXER_TRANSITION` moves from `0` to `500` over `mixer_vtol_transition_scale_ramp_time_ms`. +- During MC -> FW, `INPUT_MIXER_TRANSITION` must not move backwards when the FW profile is selected. +- During FW -> MC, it must also continue smoothly if the transition is aborted or reversed. +- If the destination profile does not own the same tilt servo output, INAV keeps the current servo output and moves it toward the destination output instead of briefly jumping through a middle/default position. + +Example: + +- MC tilt position: `90 degrees` +- transition tilt position from source 38: `45 degrees` +- FW profile final position: `0 degrees` +- `mixer_vtol_transition_scale_ramp_time_ms = 1200` + +Expected movement: + +- The servo moves from `90` toward `45` over about `1.2s`. +- If the profile switch then changes the final target from `45` to `0`, the servo continues from its current output and moves toward `0` over a fresh `1.2s`. + +### Optional fixed-wing control preview on MC profile + +If your aircraft has control surfaces that should start helping before the profile switch, add `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules in the MC mixer profile. + +Useful inputs: -### Per-mixer-profile settings +- `INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL` (`61`) +- `INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH` (`62`) +- `INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW` (`63`) +- positive-only and negative-only variants `64..69` -In the examples in this guide, mixer profile 1 is FW and mixer profile 2 is MC. -These settings can differ between the two mixer profiles: +During MC -> FW, these rules let the MC profile apply a preview of the target fixed-wing stabilisation. The preview uses the target fixed-wing PID bank, rates, angle limits, heading-hold limits, and turn-assist gains. During FW -> MC, the same MC profile rules mark which FW servo outputs should reduce their fixed-wing correction while MC motor control comes back. -- `mixer_automated_switch` -- `mixer_switch_trans_timer` -- `mixer_vtol_transition_dynamic_mixer` -- `mixer_vtol_manualswitch_autotransition_controller` -- `mixer_vtol_transition_airspeed_timeout_ms` -- `mixer_vtol_transition_scale_ramp_time_ms` - -### Global settings +If you do not configure these rules, pusher and tilt transition can still work. This preview is optional and mainly helps aircraft where control surfaces should start contributing before the final profile switch. -These are shared system-wide and are not profile-specific: +## 3. Automated mission transition (fully autonomous flight) -- `vtol_transition_to_fw_min_airspeed_cm_s` -- `vtol_transition_to_mc_max_airspeed_cm_s` -- `vtol_fw_to_mc_auto_switch_airspeed_cm_s` -- `vtol_transition_lift_min_percent` -- `vtol_transition_mc_authority_min_percent` -- `vtol_transition_fw_authority_min_percent` -- `nav_vtol_mission_transition_user_action` -- `nav_vtol_mission_transition_min_altitude_cm` +Mission transition lets a waypoint request MC or FW mode. The mission pauses while INAV performs the transition, then resumes after the transition completes. -## CLI Commands (English) +### Required setup -Use these commands in CLI (`set ...`, then `save`): +Enable navigation-requested profile changes in profiles where NAV is allowed to change mode: -- `set mixer_vtol_manualswitch_autotransition_controller = ON|OFF` - - Makes `MIXER TRANSITION` start one automatic transition each time you turn it ON. +``` +mixer_profile 1 +set mixer_automated_switch = ON + +mixer_profile 2 +set mixer_automated_switch = ON +save +``` + +Enable the waypoint USER flag that will select the VTOL mode: + +``` +set nav_vtol_mission_transition_user_action = USER1 +set nav_vtol_mission_transition_min_altitude_cm = 1200 +set nav_vtol_transition_retry_on_airspeed_timeout = ON +set nav_vtol_transition_fail_action_mc_to_fw = POSH +set nav_vtol_transition_fail_action_fw_to_mc = LOITER +save +``` + +How waypoint USER selection works: + +- selected USER bit clear (`0`) means target MC profile +- selected USER bit set (`1`) means target FW profile +- every navigable waypoint should intentionally set or clear that USER bit +- if the aircraft is already in the requested profile, INAV continues without starting a transition + +Example mission: + +- WP1: USER1 not set, climb in MC near the takeoff area +- WP2: USER1 set, request MC -> FW transition +- WP3: USER1 set, continue fixed-wing navigation +- LAND or final approach waypoint: USER1 not set, request FW -> MC before landing + +For MC -> FW mission transition, INAV uses a straight acceleration segment. It does not try to loiter to build airspeed. Normal waypoint advancement is paused until the transition is finished. + +### Altitude settings that matter in missions + +`nav_vtol_mission_transition_min_altitude_cm` + +- Current purpose: minimum altitude before a mission-requested VTOL transition may start. +- New transition use: prevents a mission from starting MC -> FW too low. +- `0` disables this gate. + +Example: + +- `nav_vtol_mission_transition_min_altitude_cm = 1200` +- Mission MC -> FW transition waits until current altitude is at least `12m` above the navigation reference. + +`nav_wp_enforce_altitude` + +- Existing purpose: controls whether a waypoint is considered reached only by horizontal distance, or also by altitude. +- New VTOL mission use: helps prevent the aircraft from accepting a transition waypoint horizontally while still far below the requested waypoint altitude. + +Examples: + +- `nav_wp_enforce_altitude = 0`: waypoint can be accepted by horizontal radius even if altitude is still not reached. This is faster, but can be risky for VTOL transition missions. +- Example with `nav_wp_enforce_altitude = 0`: if WP1 is directly above home at `120m`, the aircraft can be inside the horizontal waypoint radius immediately after takeoff. The waypoint may be accepted before the aircraft climbs to `120m`, so the mission can continue to the next action too early. +- `nav_wp_enforce_altitude = 100`: waypoint must be within about `1m` of the target altitude before it is considered reached. +- `nav_wp_enforce_altitude = 300`: waypoint must be within about `3m`; this is more relaxed and can be useful when altitude estimates are noisy. + +`nav_wp_radius` + +- Existing purpose: horizontal radius used to decide whether a normal waypoint has been reached. +- VTOL mission impact: if this is very large, a waypoint can be accepted early. Use `nav_wp_enforce_altitude` and `nav_vtol_mission_transition_min_altitude_cm` to avoid starting a transition before the aircraft is high enough. +- Landing safety use: VTOL MC landing settle uses a stricter internal landing capture radius based on `min(nav_wp_radius, 100cm)`. + +Examples: + +- `nav_wp_radius = 200`: waypoint accepted within `2m` horizontally. +- `nav_wp_radius = 1000`: waypoint accepted within `10m`; useful for fast FW navigation, but too loose by itself for deciding that a VTOL has settled at a landing point. + +### Mission transition failure behavior + +If transition cannot start because of temporary runtime conditions, INAV waits instead of immediately failing the mission. Temporary conditions include sensor calibration still in progress, navigation position temporarily unusable, heading temporarily unusable, or another transition already active. + +Hard safety/configuration problems are treated as errors or use the configured fail action where applicable. Examples include disarmed state, failsafe, missing mixer-profile mode setup, or no valid profile switch path. + +MC -> FW fail action: + +- `IDLE`: stop mission navigation and leave the pilot to recover +- `POSH`: enter position hold if possible +- `RTH`: return to home +- `EMERGENCY_LANDING`: start emergency landing behavior + +FW -> MC fail action: + +- `IDLE`: stop mission navigation and leave the pilot to recover +- `LOITER`: loiter in fixed-wing if possible +- `RTH`: return to home +- `EMERGENCY_LANDING`: start emergency landing behavior +- `FORCE_SWITCH`: force the target mixer switch even though the normal transition condition failed + +Use `FORCE_SWITCH` only when you have tested the airframe carefully. It is intended as a last-resort choice for setups where staying in FW is more dangerous than switching. + +`nav_vtol_transition_retry_on_airspeed_timeout = ON` allows one retry after an airspeed timeout. If the retry also fails, the configured fail action is used. + +## 4. Adding VTOL stabilisation + +A VTOL in MC mode is not always equivalent to a normal multicopter. Large wings can still create lift and drag while the aircraft is braking, yawing, descending, or correcting altitude. That can turn normal MC commands into unexpected climb, sink, roll, pitch, or yaw coupling. + +The VTOL MC protection feature is disabled by default: + +``` +set vtol_mc_protection_mode = OFF +``` + +This means: + +- normal multicopters do not change +- fixed-wing mode does not change +- existing VTOL behavior does not change unless you enable the feature + +### Protection modes + +`vtol_mc_protection_mode = OFF` + +- Current behavior: no new VTOL MC protection. +- Use this when comparing against older behavior. + +`vtol_mc_protection_mode = NAV` + +- Adds VTOL MC protections during navigation and automatic-throttle operation. +- Applies only when INAV detects an active multicopter-like profile and another configured profile is fixed-wing-like. +- Does not affect fixed-wing flight. +- Does not affect normal non-VTOL multicopters. + +With `NAV`, INAV can: + +- reserve throttle range for attitude stabilisation before altitude controller anti-windup limits are applied +- keep hover throttle inside the safe range, shrinking the reserve if necessary +- damp horizontal speed before fully settling position-hold behavior +- relax altitude target capture while the aircraft is bleeding speed or transitioning in MC +- require a more stable landing condition before starting/describing landing as complete +- use a conservative recovery path if attitude becomes excessive during automatic-throttle VTOL MC navigation + +`vtol_mc_protection_mode = NAV_AND_STABILIZED` -- `set mixer_vtol_transition_dynamic_mixer = ON|OFF` - - Turns smooth transition power changes ON or OFF. +- Includes everything from `NAV`. +- Also shapes pilot roll, pitch, and yaw commands in ANGLE and HORIZON mode when the aircraft is armed, in VTOL MC mode, not fixed-wing, velocity estimate is trusted, and horizontal speed is high enough. +- This is the mode to use when large wings make manual ANGLE/HORIZON MC flight feel too aggressive at forward speed. -- `set vtol_transition_to_fw_min_airspeed_cm_s = ` - - Preferred MC -> FW completion threshold (pitot airspeed). +### Roll, pitch, and yaw command shaping -- `set mixer_switch_trans_timer = ` - - Backup transition time used when pitot airspeed is not available. +The amount of shaping changes continuously with horizontal speed: -- `set vtol_transition_to_mc_max_airspeed_cm_s = ` - - FW -> MC completion threshold (pitot airspeed). +- below about `300 cm/s`, commands are unchanged +- between about `300 cm/s` and `800 cm/s`, commands are gradually reduced +- above about `800 cm/s`, commands are limited to about 50% of the original command -- `set vtol_fw_to_mc_auto_switch_airspeed_cm_s = ` - - Optional low-speed protection threshold for fixed-wing. After it switches to MC, iNAV stays in MC until you deliberately command another manual profile change (`0` disables). +Example: + +- At `250 cm/s`, yaw/roll/pitch stick response is normal. +- At `550 cm/s`, the same stick movement commands a smaller rate/angle than usual. +- At `900 cm/s`, the same stick movement commands about half of the normal response. + +This is intended to reduce wing-driven surprises during fast MC-mode flight, not to make the aircraft unresponsive. If velocity estimate is not trusted, this shaping is not applied. + +### Throttle reserve for attitude authority + +`vtol_mc_thr_reserve_percent` + +- Current purpose: reserves throttle range so altitude control cannot consume all motor authority. +- Applies when VTOL MC protection is active. +- The reserve is applied before altitude controller anti-windup bounds, not only as a final output clamp. +- Hover throttle is kept inside the safe range. If the configured reserve would exclude hover throttle, INAV shrinks the reserve and sets a debug flag. + +Example: + +- `nav_mc_hover_thr = 1500` +- `vtol_mc_thr_reserve_percent = 15` +- altitude control is not allowed to command all the way to minimum throttle, because very low base throttle can leave roll, pitch, and yaw corrections with little effective motor authority +- altitude control is not allowed to command all the way to maximum throttle, because saturated motors have no upward headroom left for attitude correction +- roll, pitch, and yaw still have headroom for correction + +If `vtol_mc_thr_reserve_percent = 0`, the throttle range is not narrowed, but the other enabled VTOL MC protections can still run. + +### Altitude tolerance while braking or transitioning + +During VTOL MC braking, wing lift can make the aircraft climb even when the pilot or NAV only intended to slow down. If altitude hold reacts too aggressively, it can reduce throttle too far, then later command too much throttle, leaving less room for attitude stabilisation. + +With VTOL MC protection enabled, INAV is more tolerant while the aircraft is still settling: + +- NAV capture damps velocity first instead of immediately forcing a hard final hold point. +- Soft altitude capture follows current altitude more gently while horizontal speed is being reduced. +- During auto transition while the active side is MC, the same protection can reduce altitude-control aggression. +- Once the aircraft is stable, normal altitude/position behavior resumes. + +This is meant to cover the case where the wings affect MC behavior during braking, yawing, and transition. + +## 5. Landing detection setup + +Landing detection is safety-critical for VTOL. A large VTOL wing can make the aircraft bounce in ground effect, and barometric altitude alone cannot prove that the aircraft is on the ground. + +The new VTOL MC landing logic keeps the normal INAV landing detector, but adds extra checks before VTOL MC can report `LANDING_DETECTED`. + +### Basic landing settings + +`nav_rth_allow_landing` + +- Existing purpose: controls whether RTH is allowed to land. +- Values are `NEVER`, `ALWAYS`, and `FS_ONLY`. +- If landing is not allowed, RTH will not intentionally descend to land. + +`nav_disarm_on_landing` + +- Existing purpose: allows automatic disarm after landing detection. +- If OFF, landing may be detected but the FC will not automatically disarm because of landing. + +`nav_auto_disarm_delay` + +- Existing purpose: delay after landing detection before automatic disarm. +- VTOL MC use: delay still applies, but VTOL MC must first pass its additional landing confirmation checks. + +`nav_land_detect_sensitivity` + +- Existing purpose: scales the generic landing detector velocity and gyro thresholds. +- At default `5`, MC landing detection uses about `100 cm/s` horizontal speed, `100 cm/s` vertical speed, and `4 deg/s` average pitch/roll gyro threshold. +- Higher values make detection easier and earlier, but increase false-detect risk. +- VTOL MC use: this setting can create a landing candidate, but it cannot bypass VTOL MC vertical-speed and throttle-probe confirmation. + +Examples: + +- `nav_land_detect_sensitivity = 5`: conservative default. +- `nav_land_detect_sensitivity = 7`: detects more easily, useful if the craft bounces on touchdown, but test carefully. +- `nav_land_detect_sensitivity = 10`: much more relaxed; not recommended for initial VTOL testing because it can create landing candidates while still airborne. -- `set mixer_vtol_transition_airspeed_timeout_ms = ` - - How long iNAV waits for required pitot airspeed before aborting. +`nav_landing_bump_detection` -- `set mixer_vtol_transition_scale_ramp_time_ms = ` - - Ramp time for MC->FW forward motor power, FW->MC forward motor removal, and the short post-switch fade of old lift/pusher outputs. +- Existing purpose: allows a touchdown acceleration bump to become a landing candidate. +- VTOL MC use: a bump is not an immediate disarm shortcut. Trusted high AGL blocks it, and accepted candidates must still pass throttle-probe confirmation. -- `set vtol_transition_lift_min_percent = <0..100>` - - Lowest lift motor power used during transition. +### Descent speed and final landing behavior -- `set vtol_transition_mc_authority_min_percent = <0..100>` - - Lowest multicopter stabilisation used during transition. +`nav_land_maxalt_vspd` -- `set vtol_transition_fw_authority_min_percent = <0..100>` - - Lowest fixed-wing control used during transition. +- Existing purpose: requested vertical descent speed above `nav_land_slowdown_maxalt` during RTH landing. -- `set nav_vtol_mission_transition_user_action = OFF|USER1|USER2|USER3|USER4` - - Selects which waypoint USER flag tells iNAV to use MC or FW at each waypoint. +`nav_land_minalt_vspd` + +- Existing purpose: requested vertical descent speed under `nav_land_slowdown_minalt` during RTH landing. +- VTOL MC use: also provides the conservative vertical-speed reference for final landing settle checks, capped internally so it cannot become too loose. +- This is used because it represents the descent speed you already consider acceptable near the ground. + +`nav_land_slowdown_maxalt` + +- Existing purpose: altitude where RTH landing starts slowing from `nav_land_maxalt_vspd` toward `nav_land_minalt_vspd`. + +`nav_land_slowdown_minalt` + +- Existing purpose: altitude where RTH landing should already be using `nav_land_minalt_vspd`. + +Examples: + +- `nav_land_minalt_vspd = 50`: final descent target is `0.5 m/s`; VTOL final landing checks stay conservative. +- `nav_land_minalt_vspd = 100`: final descent target is `1.0 m/s`; VTOL final landing checks allow a faster near-ground descent, but remain internally capped. +- `nav_land_maxalt_vspd = 200`: descent above the slowdown window can be `2.0 m/s`. +- `nav_land_slowdown_maxalt = 2000` and `nav_land_slowdown_minalt = 500`: descent slows between `20m` and `5m` above the landing reference. + +### VTOL MC throttle-probe confirmation + +VTOL MC landing candidates must pass a short lift-throttle confirmation. + +In plain language: + +- INAV sees a possible landing. +- Instead of immediately reporting landed, it gently reduces lift throttle for a short confirmation window. +- If the aircraft starts falling away, AGL drops, low-G/unloading is detected, or vertical speed changes toward a stronger descent, the landing candidate is rejected. +- If that small throttle reduction does not produce airborne-response evidence during the confirmation window, landing can be reported. + +This avoids a common false positive: the aircraft is still high in the air, vertical speed is temporarily low, and gyro rates are calm, so the generic detector thinks it might be landed. A real airborne VTOL should react to a small lift-throttle reduction; a landed aircraft should not fall away like an airborne one. + +The confirmation is not meant to reject every bounce, rocking motion, or pitch/roll wobble by itself. A VTOL can bounce in ground effect shortly before real touchdown. The important question for this check is whether reducing lift throttle causes the aircraft to continue descending like it is still flying. If it does, INAV rejects the candidate and waits for another landing opportunity. + +The confirmation does not rely on barometric altitude as proof of AGL. If a trusted surface/AGL sensor is available, it is used as an additional safety input. Without trusted AGL, vertical motion and acceleration are more important than baro altitude. + +`nav_mc_hover_thr` matters here, but the probe does not assume that landing throttle is equal to hover throttle. It starts from the current adjusted throttle at the moment of the landing candidate, which during descent may already be below `nav_mc_hover_thr`. `nav_mc_hover_thr` is used to size a small bounded throttle reduction relative to idle/hover range. If the current landing throttle is already below that probe limit, INAV does not raise it just to run the probe. Tune hover throttle before relying on automatic VTOL landing because it still affects throttle reserve and the probe reduction size. + +### Landing settle before descent + +VTOL MC landing should not start descent just because the aircraft briefly touches a large waypoint radius. + +For VTOL MC landing settle: + +- INAV uses a tighter landing capture radius based on `min(nav_wp_radius, 100cm)`. +- Horizontal speed must be low. +- Vertical speed must be low when a vertical estimate is available. +- Roll/pitch attitude must be within a safe range. +- Conditions must stay stable long enough before landing descent/landing detection is allowed to proceed. + +Examples: + +- `nav_wp_radius = 500`: normal waypoint acceptance is `5m`, but VTOL landing settle uses `1m`. +- `nav_wp_radius = 80`: VTOL landing settle uses `80cm`. +- `nav_wp_radius = 1500`: normal waypoint acceptance is `15m`, but VTOL landing settle still uses `1m`. + +This lets fixed-wing missions keep a larger waypoint radius while preventing VTOL landing from starting just because the aircraft briefly crossed a loose radius. + +### Practical landing setup + +Conservative starting point: + +``` +set nav_rth_allow_landing = ALWAYS +set nav_disarm_on_landing = ON +set nav_auto_disarm_delay = 1000 +set nav_land_detect_sensitivity = 5 +set nav_landing_bump_detection = ON +set nav_land_minalt_vspd = 50 +set nav_land_maxalt_vspd = 150 +set nav_land_slowdown_maxalt = 2000 +set nav_land_slowdown_minalt = 500 +set vtol_mc_protection_mode = NAV +save +``` -- `set nav_vtol_mission_transition_min_altitude_cm = ` - - Optional minimum altitude before mission transition may start (`0` disables). +If the aircraft bounces on landing: + +- Do not immediately raise `nav_land_detect_sensitivity` a lot. +- First reduce final descent energy: lower `nav_land_minalt_vspd` if possible, tune hover throttle, and verify `nav_mc_hover_thr`. +- Keep `nav_landing_bump_detection = ON` so a real touchdown bump can help create a candidate. +- Increase `nav_land_detect_sensitivity` only in small steps, and verify with blackbox logs. + +## Debugging and setting scope + +Useful debug modes: + +- `set debug_mode = VTOL_TRANSITION`: transition phase, transition direction, progress, motor scaling, servo transition progress, and profile-switch smoothing state. +- `set debug_mode = VTOL_MC_PROTECT`: VTOL MC protection flags, safe throttle min/max, protected throttle, speed, attitude, and command-shaping/settle progress. +- `set debug_mode = LANDING`: normal landing detector path and landing candidate state. + +`VTOL_TRANSITION` debug channels: + +- `debug[0]`: transition phase (`0=IDLE`, `1=TRANSITION_INITIALIZE`, `2=TRANSITIONING`, `3=after-switch smoothing`). +- `debug[1]`: active transition request, with direction packed in the high byte. +- `debug[2]`: packed flags. Important bits include direction, controller active, transition input active, airspeed path active, profile switch done, abort state, current/next profile index, mission active, failsafe active, direct profile switch active, target preview mode, after-switch smoothing active, and legacy manual session active. +- `debug[3]`: main transition progress x1000 (`0..1000`). +- `debug[4]`: pusher/forward motor scale x1000 (`0..1000`). +- `debug[5]`: lift motor scale x1000 (`0..1000`). +- `debug[6]`: packed MC/FW stabilisation scales. Low 16 bits are MC stabilisation scale x1000, high 16 bits are FW control scale x1000. +- `debug[7]`: packed progress values. Bits `0..9` are airspeed-linked scaling progress, bits `10..19` are motor ramp progress, and bits `20..29` are after-switch smoothing progress. + +`VTOL_MC_PROTECT` debug channels: + +- `debug[0]`: flags bitmask. Bits show protection configured, VTOL MC detected, NAV protection active, ANGLE/HORIZON protection active, NAV capture active, landing settle active, bailout active, throttle reserve shrunk, soft altitude capture active, roll/pitch/yaw command shaped, and velocity fallback used. +- `debug[1]`: safe throttle minimum. +- `debug[2]`: safe throttle maximum. +- `debug[3]`: protected throttle. +- `debug[4]`: horizontal speed [cm/s]. +- `debug[5]`: vertical speed [cm/s]. +- `debug[6]`: max absolute roll/pitch attitude [deci-degrees]. +- `debug[7]`: capture/landing/bailout settle elapsed time [ms], or command scale x1000 when command shaping is active, otherwise `1000`. + +Per-mixer-profile settings: + +- `mixer_automated_switch`: existing RTH use is to allow NAV to switch between MC and FW for return/landing. New mission use is to allow waypoint USER actions to request MC/FW transitions. +- `mixer_switch_trans_timer`: existing legacy transition timer in deciseconds. New auto-transition use is the fallback completion timer when pitot airspeed is not used or not trusted. +- `mixer_vtol_transition_dynamic_mixer`: new optional smooth power/control scaling. OFF keeps old transition motor/servo behavior; ON allows smooth pusher, lift, MC stabilisation, FW control, and transition-servo movement. +- `mixer_vtol_manualswitch_autotransition_controller`: new manual switch controller. OFF keeps older manual behavior; ON makes `MIXER TRANSITION` start one complete transition. +- `mixer_vtol_transition_airspeed_timeout_ms`: new airspeed wait limit. It does not complete transition; it aborts an airspeed-controlled attempt that takes too long while pitot remains trusted. +- `mixer_vtol_transition_scale_ramp_time_ms`: new time used for smooth pusher/lift/transition-servo movement. It also controls how long transition-linked servo outputs continue from their current position after a direct switch or abort. + +Global VTOL transition settings: + +- `vtol_transition_to_fw_min_airspeed_cm_s`: new preferred MC -> FW completion threshold when pitot is trusted. `0` uses the timer path. +- `vtol_transition_to_mc_max_airspeed_cm_s`: new preferred FW -> MC completion threshold when pitot is trusted. `0` uses the timer path. +- `vtol_fw_to_mc_auto_switch_airspeed_cm_s`: new low-speed fixed-wing protection. When non-zero, FW can automatically start FW -> MC if pitot airspeed falls too low. +- `vtol_transition_lift_min_percent`: new lowest lift motor power during dynamic transition scaling. `100` keeps full lift power. +- `vtol_transition_mc_authority_min_percent`: new lowest MC motor stabilisation strength during dynamic transition scaling. `100` keeps full MC stabilisation. +- `vtol_transition_fw_authority_min_percent`: new lowest FW control strength during dynamic transition scaling. It also scales optional target fixed-wing servo preview rules. +- `nav_vtol_mission_transition_user_action`: new mission selector. OFF disables mission transition; USER1..USER4 chooses which waypoint USER bit requests MC or FW. +- `nav_vtol_mission_transition_min_altitude_cm`: new minimum-altitude gate before mission-requested transition may start. `0` disables this gate. +- `nav_vtol_transition_retry_on_airspeed_timeout`: new mission retry option after an airspeed timeout. If retry also fails, the configured fail action is used. +- `nav_vtol_transition_fail_action_mc_to_fw`: new MC -> FW mission failure action after transition cannot complete safely. +- `nav_vtol_transition_fail_action_fw_to_mc`: new FW -> MC mission failure action after transition cannot complete safely. + +Global VTOL MC protection and landing settings: + +- `vtol_mc_protection_mode`: new master switch for VTOL MC protection. OFF changes nothing; NAV protects navigation/altitude behavior; NAV_AND_STABILIZED also shapes ANGLE/HORIZON roll, pitch, and yaw commands at speed. +- `vtol_mc_thr_reserve_percent`: new throttle reserve for attitude authority while altitude/NAV owns throttle. Applied before altitude anti-windup bounds. +- `nav_mc_hover_thr`: existing MC hover throttle hint. New VTOL protection use is to keep hover throttle inside the protected range and to make landing throttle confirmation more accurate. +- `nav_wp_radius`: existing normal waypoint acceptance radius. New VTOL landing use is capped internally for landing settle so a large waypoint radius cannot by itself start landing too early. +- `nav_wp_enforce_altitude`: existing waypoint altitude acceptance tolerance. New VTOL mission use is to keep transition waypoints from being accepted horizontally while still far from target altitude. +- `nav_rth_allow_landing`: existing RTH landing permission. VTOL landing logic only matters when navigation is actually allowed to land. +- `nav_disarm_on_landing`: existing automatic disarm permission. New VTOL MC landing confirmation must pass before this can lead to disarm. +- `nav_auto_disarm_delay`: existing delay after landing detection. VTOL MC adds its own confirmation before landing is reported. +- `nav_land_detect_sensitivity`: existing generic landing detector sensitivity. New VTOL MC use allows it to create candidates, but not to bypass vertical-speed and throttle confirmation. +- `nav_landing_bump_detection`: existing touchdown-bump candidate detection. New VTOL MC use treats bumps as candidates only, not immediate proof of landing. +- `nav_land_minalt_vspd`: existing final RTH landing descent speed. New VTOL MC use is a conservative vertical-speed reference for final settle/landing checks. +- `nav_land_maxalt_vspd`: existing higher-altitude RTH landing descent speed. It controls descent energy before the final slowdown region. +- `nav_land_slowdown_minalt`: existing lower boundary of the RTH landing slowdown window. Below this, RTH landing should be using `nav_land_minalt_vspd`. +- `nav_land_slowdown_maxalt`: existing upper boundary of the RTH landing slowdown window. Above this, RTH landing may use `nav_land_maxalt_vspd`. Mission profile-switch dependency: -- Mission VTOL transition uses the existing profile-change path, so two valid mixer profiles and a configured `MIXER PROFILE 2` mode activation condition are required. +- Mission VTOL transition uses the existing profile-change path. +- Configure two valid mixer profiles. +- Configure a valid `MIXER PROFILE 2` mode activation condition. +- Enable `mixer_automated_switch` in profiles where NAV is allowed to request VTOL profile changes. # Notes and Experiences ## General @@ -672,19 +1058,21 @@ Smooth transition power changes (`mixer_vtol_transition_dynamic_mixer = ON`) use - MC stabilisation ramps `vtol_transition_mc_authority_min_percent -> 1` - FW control ramps `1 -> vtol_transition_fw_authority_min_percent` -After the profile switch, iNAV keeps only the old propulsion output alive for a short fade: +After the profile switch, INAV keeps only the old propulsion output alive for a short smooth shutdown: -- MC -> FW: lift motors that are not used by the FW profile fade to idle. -- FW -> MC: a forward motor that is not used by the MC profile keeps fading to idle. -- This post-switch fade uses `mixer_vtol_transition_scale_ramp_time_ms` and does not keep the old PID/controller active. +- MC -> FW: lift motors that are not used by the FW profile move to idle. +- FW -> MC: a forward motor that is not used by the MC profile keeps moving to idle. +- This after-switch smoothing uses `mixer_vtol_transition_scale_ramp_time_ms` and does not keep the old PID/controller active. -Motor ramp-in and control handover are separate. +Forward motor power increase, transition servo movement, and airspeed-linked control scaling are related but separate. For MC->FW, forward motor power uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, the motor goes to full power immediately. For FW->MC, the same timer ramps the forward motor down to idle while lift power and MC stabilisation rise back from their configured minimums; if this is `0`, those changes happen immediately. +For `INPUT_MIXER_TRANSITION`, the same timer is used only when `mixer_vtol_transition_dynamic_mixer = ON`. With dynamic mixer OFF, source 38 keeps the older fixed transition endpoint behavior. +If a profile switch or direct switch affects servos that use transition-linked inputs, INAV captures the current servo output and moves from that captured output toward the new profile output using a fresh `mixer_vtol_transition_scale_ramp_time_ms` window. This timer does not decide when the transition completes. -In MC->FW, lift power reduction, MC stabilisation fade-out, and FW control fade-in still prefer pitot-based transition progress whenever pitot is working. -In FW->MC, FW control fade-out still prefers pitot-based transition progress, while forward motor removal, lift power ramp-in, and MC motor stabilisation ramp-in use the time-based motor ramp. -If pitot is not usable, the airspeed-based handoff changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). +In MC->FW, lift power reduction, MC stabilisation reduction, and FW control increase still prefer pitot-based transition progress whenever pitot is working. +In FW->MC, FW control reduction still prefers pitot-based transition progress, while forward motor removal, lift power return, and MC motor stabilisation return use the time-based motor ramp. +If pitot is not usable, the airspeed-linked changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). For legacy/helper transition motors (`-2.0 <= throttle <= -1.05`), output is interpolated from idle to target: @@ -696,4 +1084,4 @@ where: If pitot is unavailable/unhealthy, timer fallback is used (`mixer_switch_trans_timer`). -For smooth auto-transition, the preferred forward motor setup is a normal positive-throttle rule in the FW mixer profile, with a placeholder on the same motor index in the MC mixer profile. Use `throttle = -1.000` for that placeholder if Configurator removes zero-throttle motor rules. If a helper such as `throttle = -1.200` is used, iNAV fades from that helper output to the real FW mixer output after the hot-switch. +For smooth auto-transition, the preferred forward motor setup is a normal positive-throttle rule in the FW mixer profile, with a placeholder on the same motor index in the MC mixer profile. Use `throttle = -1.000` for that placeholder if Configurator removes zero-throttle motor rules. If a helper such as `throttle = -1.200` is used, INAV moves from that helper output to the real FW mixer output after the profile switch. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7151e693c28..0773d3a22c6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1275,18 +1275,18 @@ groups: field: mixer_config.controlProfileLinking type: bool - name: mixer_automated_switch - description: "If set to on, This mixer_profile will try to switch to another mixer_profile when 1.RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius on mixer_profile is a MULTIROTOR or TRICOPTER platform_type. 2. RTH landing is requested on this mixer_profile is a AIRPLANE platform_type" + description: "Allows navigation code to request VTOL mixer profile changes from this profile. In legacy RTH this lets MC/TRICOPTER profiles switch toward FW when returning from far away, and lets AIRPLANE profiles switch toward MC for RTH landing. With the VTOL auto-transition controller, mission waypoints can also request MC/FW transitions through their USER action. Enable this in each profile where NAV is allowed to change the mixer profile." default_value: OFF field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, the control-handoff side of the transition also falls back to this timing whenever trusted pitot is not usable." + description: "Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, INAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, airspeed-linked power and control changes also fall back to this timer whenever trusted pitot is not usable." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 max: 200 - name: mixer_vtol_transition_dynamic_mixer - description: "Turns on smooth VTOL transition handoff scaling. In MC->FW it ramps the forward motor up while lift power and multicopter stabilisation fade down and fixed-wing control fades in. In FW->MC it ramps the forward motor down while target MC lift power and target MC motor stabilisation ramp back in, and fixed-wing control fades down. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash." + description: "Turns on smooth VTOL transition power and control scaling. In MC->FW it smoothly brings the forward motor in while lift power and multicopter stabilisation are reduced and fixed-wing control is increased. In FW->MC it smoothly removes the forward motor while target MC lift power and target MC motor stabilisation come back in, and fixed-wing control is reduced. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: OFF field: mixer_config.vtolTransitionDynamicMixer @@ -1298,14 +1298,14 @@ groups: field: mixer_config.manualVtolTransitionController type: bool - name: mixer_vtol_transition_airspeed_timeout_ms - description: "Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. Available only on targets with more than 512 KB flash." + description: "Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, INAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 0 field: mixer_config.vtolTransitionAirspeedTimeoutMs min: 0 max: 60000 - name: mixer_vtol_transition_scale_ramp_time_ms - description: "When smooth VTOL transition power changes are ON, this controls the time-based motor/power handover only. In MC->FW it ramps the forward motor from idle to full target power. After the switch to FW, any old lift motors that are no longer used by the FW profile fade to idle over the same time. In FW->MC it ramps the forward motor down to idle and ramps target MC lift power plus target MC motor stabilisation back in. After the switch to MC, any old forward motor that is no longer used by the MC profile is kept fading to idle. `0` applies those time-based power changes immediately. This timer does not decide when the transition completes and it does not control fixed-wing control-surface handoff. Fixed-wing handoff still follows trusted pitot airspeed when pitot is usable, otherwise `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash." + description: "Controls time-based smooth VTOL motor, power, and transition-servo movement. In MC->FW it moves the forward motor from idle to the requested power. After the switch to FW, old lift motors that are no longer used by the FW profile are moved to idle over the same time. In FW->MC it moves the forward motor down to idle while target MC lift power plus target MC motor stabilisation come back in. `INPUT_MIXER_TRANSITION` also uses this time when `mixer_vtol_transition_dynamic_mixer` is ON. If a profile switch or direct switch changes a transition-linked servo output, INAV starts a fresh smooth movement from the servo's current output using this full time value. `0` applies these time-based changes immediately. This timer does not decide when transition completes; completion still uses trusted pitot airspeed or `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 0 field: mixer_config.vtolTransitionScaleRampTimeMs @@ -2593,13 +2593,13 @@ groups: field: general.flags.disarm_on_landing type: bool - name: nav_land_detect_sensitivity - description: "Changes sensitivity of landing detection. Higher values increase speed of detection but also increase risk of false detection. Default value should work in most cases." + description: "Scales the generic landing detector velocity and gyro thresholds. `5` is nominal sensitivity: for MC this is about 100 cm/s horizontal and vertical speed plus 4 deg/s average pitch/roll gyro rate. Higher values relax these thresholds and can detect landing sooner, but increase false-detect risk. VTOL MC landing detection also uses additional hard vertical-speed and throttle-probe safety gates that this setting does not bypass." default_value: 5 field: general.land_detect_sensitivity min: 1 max: 15 - name: nav_landing_bump_detection - description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS)." + description: "Allows landing detection to use a touchdown G-bump candidate when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS). For VTOL MC mode the G-bump is not an immediate disarm shortcut: trusted high AGL blocks it, and accepted candidates must still pass the VTOL MC throttle-probe confirmation." default_value: OFF field: general.flags.landing_bump_detection type: bool @@ -2659,7 +2659,7 @@ groups: field: general.flags.waypoint_mission_restart table: nav_wp_mission_restart - name: nav_vtol_mission_transition_user_action - description: "Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles, a working `MIXER PROFILE 2` mode setup, and a target with more than 512 KB flash." + description: "Chooses which waypoint USER flag (`USER1`..`USER4`) tells INAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles, a working `MIXER PROFILE 2` mode setup, and a target with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: "OFF" field: general.vtol_mission_transition_user_action @@ -2672,19 +2672,19 @@ groups: min: 0 max: 50000 - name: nav_vtol_transition_retry_on_airspeed_timeout - description: "If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. Available only on targets with more than 512 KB flash." + description: "If ON, INAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: OFF field: general.vtol_transition_retry_on_airspeed_timeout type: bool - name: nav_vtol_transition_fail_action_mc_to_fw - description: "What iNAV should do if MC->FW transition still fails after the final attempt. Available only on targets with more than 512 KB flash." + description: "What INAV should do if MC->FW transition still fails after the final attempt. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: "IDLE" field: general.vtol_transition_fail_action_mc_to_fw table: nav_vtol_transition_fail_action_mc_to_fw - name: nav_vtol_transition_fail_action_fw_to_mc - description: "What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. Available only on targets with more than 512 KB flash." + description: "What INAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: "LOITER" field: general.vtol_transition_fail_action_fw_to_mc @@ -4034,49 +4034,49 @@ groups: min: 0 max: 100 - name: vtol_transition_to_fw_min_airspeed_cm_s - description: "Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." + description: "Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 0 field: vtolTransitionToFwMinAirspeed min: 0 max: 20000 - name: vtol_transition_to_mc_max_airspeed_cm_s - description: "When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." + description: "When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 0 field: vtolTransitionToMcMaxAirspeed min: 0 max: 20000 - name: vtol_fw_to_mc_auto_switch_airspeed_cm_s - description: "Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. Available only on targets with more than 512 KB flash." + description: "Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, INAV automatically starts FW->MC. After the switch to MC, INAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 0 field: vtolFwToMcAutoSwitchAirspeed min: 0 max: 20000 - name: vtol_transition_lift_min_percent - description: "Lowest lift motor power used during transition, in percent. In MC->FW, lift power fades down to this value as the handoff progresses. In FW->MC, lift power starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + description: "Lowest lift motor power used during transition, in percent. In MC->FW, lift power is reduced to this value as the transition progresses. In FW->MC, lift power starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionLiftMinPercent min: 0 max: 100 - name: vtol_transition_mc_authority_min_percent - description: "Lowest multicopter stabilisation used during transition, in percent. In MC->FW, active MC motor stabilisation fades down to this value as the handoff progresses. In FW->MC, target MC motor stabilisation starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + description: "Lowest multicopter stabilisation used during transition, in percent. In MC->FW, active MC motor stabilisation is reduced to this value as the transition progresses. In FW->MC, target MC motor stabilisation starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionMcAuthorityMinPercent min: 0 max: 100 - name: vtol_transition_fw_authority_min_percent - description: "Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength as the handoff progresses. In FW->MC, it fades down from full strength to this value as the handoff progresses. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + description: "Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength as the transition progresses. In FW->MC, it is reduced from full strength to this value as the transition progresses. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 100 field: vtolTransitionFwAuthorityMinPercent min: 0 max: 100 - name: vtol_mc_protection_mode - description: "Enables extra protection for VTOL aircraft while the active mixer profile is multicopter-like and another mixer profile is fixed-wing. OFF keeps legacy behavior. NAV protects altitude/NAV throttle authority, NAV capture, landing settle, and conservative bailout. NAV_AND_STABILIZED also applies speed-based roll/pitch/yaw command shaping in ANGLE/HORIZON. Available only on targets with more than 512 KB flash." + description: "Enables extra protection for VTOL aircraft while the active mixer profile is multicopter-like and another mixer profile is fixed-wing. OFF keeps legacy behavior. NAV protects altitude/NAV throttle authority, NAV capture/settle, landing settle, VTOL MC landing confirmation, and conservative bailout. NAV_AND_STABILIZED also applies speed-based roll/pitch/yaw command shaping in ANGLE/HORIZON when armed, VTOL MC mode is detected, velocity is trusted, and horizontal speed is high enough. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: "OFF" field: vtolMcProtectionMode diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ceb8a625059..f3446e28fd7 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -57,6 +57,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" #include "navigation/navigation_vtol_mc_protection.h" +#include "navigation/navigation_vtol_mission_logic.h" #include "navigation/rth_trackback.h" #include "rx/rx.h" @@ -2402,14 +2403,21 @@ static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const const uint16_t transitionMinAltitude = navConfig()->general.vtol_mission_transition_min_altitude; - if (!ARMING_FLAG(ARMED) || - FLIGHT_MODE(FAILSAFE_MODE) || - areSensorsCalibrating() || - posControl.flags.estPosStatus < EST_USABLE || - posControl.flags.estHeadingStatus < EST_USABLE || - !isModeActivationConditionPresent(BOXMIXERPROFILE) || - !checkMixerProfileHotSwitchAvalibility() || - mixerATIsActive()) { + const navMissionVtolTransitionPrecondition_e precondition = navMissionVtolTransitionPreconditionDisposition( + ARMING_FLAG(ARMED), + FLIGHT_MODE(FAILSAFE_MODE), + areSensorsCalibrating(), + posControl.flags.estPosStatus >= EST_USABLE, + posControl.flags.estHeadingStatus >= EST_USABLE, + isModeActivationConditionPresent(BOXMIXERPROFILE), + checkMixerProfileHotSwitchAvalibility(), + mixerATIsActive()); + + if (precondition == NAV_MISSION_VTOL_PRECONDITION_WAIT) { + return NAV_MISSION_VTOL_TRANSITION_WAIT; + } + + if (precondition == NAV_MISSION_VTOL_PRECONDITION_REJECT) { return NAV_MISSION_VTOL_TRANSITION_REJECT; } @@ -2475,6 +2483,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav case NAV_WP_ACTION_WAYPOINT: case NAV_WP_ACTION_LAND: { const navWaypoint_t * const activeWaypoint = &posControl.waypointList[posControl.activeWaypointIndex]; + + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + calculateAndSetActiveWaypoint(activeWaypoint); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 61c6d122744..1512d641dcb 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -65,6 +65,75 @@ static pt1Filter_t altholdThrottleFilterState; static bool prepareForTakeoffOnReset = false; static sqrt_controller_t alt_hold_sqrt_controller; +// A winged VTOL can satisfy passive landed thresholds while still airborne. +// Confirm touchdown by lightly reducing lift throttle and watching for descent. +typedef struct mcLandingProbeState_s { + bool active; + timeMs_t startedAtMs; + int16_t startThrottle; + bool startAglTrusted; + float startAglCm; + float startVerticalSpeedCmS; +} mcLandingProbeState_t; + +static mcLandingProbeState_t mcLandingProbe; + +static void resetMulticopterLandingProbe(void) +{ + mcLandingProbe = (mcLandingProbeState_t){ 0 }; +} + +static void startMulticopterLandingProbe(timeMs_t currentTimeMs) +{ + mcLandingProbe.active = true; + mcLandingProbe.startedAtMs = currentTimeMs; + mcLandingProbe.startThrottle = rcCommandAdjustedThrottle; + mcLandingProbe.startAglTrusted = posControl.flags.estAglStatus == EST_TRUSTED; + mcLandingProbe.startAglCm = posControl.actualState.agl.pos.z; + mcLandingProbe.startVerticalSpeedCmS = navGetCurrentActualPositionAndVelocity()->vel.z; +} + +static bool updateMulticopterLandingProbe(timeMs_t currentTimeMs, bool *probeAborted) +{ + *probeAborted = false; + + if (!mcLandingProbe.active) { + startMulticopterLandingProbe(currentTimeMs); + return false; + } + + if (vtolMcProtectionLandingProbeAirborneResponse( + mcLandingProbe.startAglTrusted && posControl.flags.estAglStatus == EST_TRUSTED, + mcLandingProbe.startAglCm, + posControl.actualState.agl.pos.z, + posControl.flags.estAltStatus >= EST_USABLE, + mcLandingProbe.startVerticalSpeedCmS, + navGetCurrentActualPositionAndVelocity()->vel.z, + acc.accADCf[Z])) { + *probeAborted = true; + resetMulticopterLandingProbe(); + return false; + } + + return currentTimeMs - mcLandingProbe.startedAtMs >= VTOL_MC_LANDING_PROBE_CONFIRM_TIME_MS; +} + +static int16_t applyMulticopterLandingProbeThrottle(const int16_t requestedThrottle, const int16_t idleThrottle, const int16_t hoverThrottle) +{ + if (!mcLandingProbe.active) { + return requestedThrottle; + } + + const int16_t probeThrottle = vtolMcProtectionLandingProbeThrottle( + mcLandingProbe.startThrottle, + idleThrottle, + hoverThrottle, + mcLandingProbe.startedAtMs, + millis()); + + return requestedThrottle < probeThrottle ? requestedThrottle : probeThrottle; +} + float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros) { return sqrtControllerApply( @@ -117,7 +186,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) const vtolMcProtectionThrottleBounds_t vtolMcThrottleBounds = navigationVtolMcProtectionGetThrottleBounds(idleThrottle, hoverThrottle, maxThrottle); const int16_t thrCorrectionMin = vtolMcThrottleBounds.min - hoverThrottle; const int16_t thrCorrectionMax = vtolMcThrottleBounds.max - hoverThrottle; - const pidControllerFlags_e pidFlags = navigationVtolMcProtectionShouldFreezeAltitudeIntegrator() ? PID_FREEZE_INTEGRATOR : 0; + const pidControllerFlags_e pidFlags = (navigationVtolMcProtectionShouldFreezeAltitudeIntegrator() || mcLandingProbe.active) ? PID_FREEZE_INTEGRATOR : 0; float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, pidFlags); @@ -126,6 +195,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) int16_t protectedThrottle = hoverThrottle + rcThrottleCorrection; protectedThrottle = navigationVtolMcProtectionApplyBailoutThrottle(protectedThrottle, &vtolMcThrottleBounds, hoverThrottle); + protectedThrottle = applyMulticopterLandingProbeThrottle(protectedThrottle, idleThrottle, hoverThrottle); posControl.rcAdjustment[THROTTLE] = setDesiredThrottle(protectedThrottle, false); navigationVtolMcProtectionPublishThrottleDebug(&vtolMcThrottleBounds, posControl.rcAdjustment[THROTTLE]); } @@ -185,6 +255,8 @@ bool adjustMulticopterAltitudeFromRCInput(void) void setupMulticopterAltitudeController(void) { + resetMulticopterLandingProbe(); + const bool throttleIsLow = throttleStickIsLow(); const uint8_t throttleType = navConfig()->mc.althold_throttle_type; @@ -211,6 +283,8 @@ void setupMulticopterAltitudeController(void) void resetMulticopterAltitudeController(void) { + resetMulticopterLandingProbe(); + const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity(); float nav_speed_up = 0.0f; float nav_speed_down = 0.0f; @@ -785,7 +859,7 @@ void updateBaroAltitudeRate(float newBaroAltRate) baroAltRate = newBaroAltRate; } -static bool isLandingGbumpDetected(timeMs_t currentTimeMs) +static bool isLandingGbumpDetected(timeMs_t currentTimeMs, bool landingBumpAllowed) { /* Detection based on G bump at touchdown, falling Baro altitude and throttle below hover. * G bump trigger: > 2g then falling back below 1g in < 0.1s. @@ -794,6 +868,11 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs) static timeMs_t gSpikeDetectTimeMs = 0; + if (!landingBumpAllowed) { + gSpikeDetectTimeMs = 0; + return false; + } + if (!gSpikeDetectTimeMs && acc.accADCf[Z] > 2.0f && baroAltRate < 0.0f) { gSpikeDetectTimeMs = currentTimeMs; } else if (gSpikeDetectTimeMs) { @@ -801,7 +880,11 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs) if (acc.accADCf[Z] < 1.0f && baroAltRate < -200.0f) { const uint16_t idleThrottle = getThrottleIdleValue(); const uint16_t hoverThrottleRange = currentBatteryProfile->nav.mc.hover_throttle - idleThrottle; - return rcCommand[THROTTLE] < idleThrottle + ((navigationInAutomaticThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange); + const bool detected = rcCommand[THROTTLE] < idleThrottle + ((navigationInAutomaticThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange); + if (detected) { + gSpikeDetectTimeMs = 0; + } + return detected; } } else if (acc.accADCf[Z] <= 1.0f) { gSpikeDetectTimeMs = 0; @@ -838,6 +921,11 @@ bool isMulticopterLandingDetected(void) DEBUG_SET(DEBUG_LANDING, 3, averageAbsGyroRates() * 100); const timeMs_t currentTimeMs = millis(); + const uint32_t navStateFlags = navGetCurrentStateFlags(); + const float verticalSpeed = navGetCurrentActualPositionAndVelocity()->vel.z; + const float absVerticalSpeed = fabsf(verticalSpeed); + const float finalDescentDemandLimit = navConfig()->general.land_minalt_vspd + MC_LAND_DETECT_DESCENT_DEMAND_MARGIN; + bool gBumpDetected = false; #if defined(USE_BARO) if (sensors(SENSOR_BARO)) { @@ -853,8 +941,20 @@ bool isMulticopterLandingDetected(void) ((posControl.flags.estPosStatus >= EST_USABLE && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING) || FLIGHT_MODE(FAILSAFE_MODE)); - if (gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs)) { - return true; // Landing flagged immediately if landing bump detected + const bool landingBumpAllowed = vtolMcProtectionLandingBumpAllowed( + navigationVtolMcProtectionIsVtolMcMode(), + navStateFlags & NAV_CTL_LAND, + posControl.flags.estAglStatus == EST_TRUSTED, + posControl.actualState.agl.pos.z, + posControl.flags.estAltStatus >= EST_USABLE, + verticalSpeed, + posControl.desiredState.vel.z, + finalDescentDemandLimit, + VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S); + + gBumpDetected = gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs, landingBumpAllowed); + if (gBumpDetected) { + DEBUG_SET(DEBUG_LANDING, 4, 3); } } #endif @@ -871,7 +971,9 @@ bool isMulticopterLandingDetected(void) throttleLowCheckAllowed = throttleLowCheckAllowed && posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 50.0f; } - bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) || + bool startCondition = mcLandingProbe.active || + gBumpDetected || + (navStateFlags & (NAV_CTL_LAND | NAV_CTL_EMERG)) || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) || (throttleLowCheckAllowed && throttleStickIsLow()); @@ -879,13 +981,12 @@ bool isMulticopterLandingDetected(void) if (!startCondition || posControl.flags.resetLandingDetector) { landingDetectorStartedAt = 0; + resetMulticopterLandingProbe(); return posControl.flags.resetLandingDetector = false; } const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f; - const float absVerticalSpeed = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z); - // check vertical and horizontal velocities are low (cm/s) bool velCondition = absVerticalSpeed < (MC_LAND_CHECK_VEL_Z_MOVING * sensitivity) && posControl.actualState.velXY < (MC_LAND_CHECK_VEL_XY_MOVING * sensitivity); @@ -899,7 +1000,13 @@ bool isMulticopterLandingDetected(void) bool possibleLandingDetected = false; - if (navGetCurrentStateFlags() & NAV_CTL_LAND) { + if (mcLandingProbe.active) { + DEBUG_SET(DEBUG_LANDING, 4, 4); + possibleLandingDetected = true; + } else if (gBumpDetected) { + DEBUG_SET(DEBUG_LANDING, 4, 3); + possibleLandingDetected = true; + } else if (navStateFlags & NAV_CTL_LAND) { // We have likely landed if throttle is 40 units below average descend throttle // We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed // from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core) @@ -924,7 +1031,6 @@ bool isMulticopterLandingDetected(void) landingThrSum += rcCommandAdjustedThrottle; isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE); - const float finalDescentDemandLimit = navConfig()->general.land_minalt_vspd + MC_LAND_DETECT_DESCENT_DEMAND_MARGIN; const bool descentDemandSettled = posControl.desiredState.vel.z > -finalDescentDemandLimit; const int16_t landingThrottleLimit = (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()) / 2; const bool throttleLowEnough = rcCommandAdjustedThrottle < landingThrottleLimit; @@ -958,11 +1064,28 @@ bool isMulticopterLandingDetected(void) * Fixed time increased if Z velocity invalid to provide extra safety margin against false triggers */ const uint16_t safetyTime = posControl.flags.estAltStatus == EST_NONE ? 5000 : 1000; timeMs_t safetyTimeDelay = safetyTime + navConfig()->general.auto_disarm_delay; - return currentTimeMs - landingDetectorStartedAt > safetyTimeDelay; + if (gBumpDetected || currentTimeMs - landingDetectorStartedAt > safetyTimeDelay) { + if (!navigationVtolMcProtectionIsVtolMcMode()) { + resetMulticopterLandingProbe(); + return true; + } + + bool probeAborted = false; + const bool probeConfirmed = updateMulticopterLandingProbe(currentTimeMs, &probeAborted); + if (probeAborted) { + landingDetectorStartedAt = currentTimeMs; + } + + return probeConfirmed; + } } else { landingDetectorStartedAt = currentTimeMs; + resetMulticopterLandingProbe(); return false; } + + resetMulticopterLandingProbe(); + return false; } bool isMulticopterThrottleAboveMidHover(void) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 56292b34813..d2077bc99c1 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -189,6 +189,8 @@ typedef enum { 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, #ifdef USE_AUTO_TRANSITION + // Only valid while NAV_STATE_MIXERAT_IN_PROGRESS is active. The same + // state-specific slot is intentionally reused by other FSM states. NAV_FSM_EVENT_MIXERAT_MISSION_RESUME = NAV_FSM_EVENT_STATE_SPECIFIC_4, #endif NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5, diff --git a/src/main/navigation/navigation_vtol_mc_protection_logic.h b/src/main/navigation/navigation_vtol_mc_protection_logic.h index a062f6a0b7a..d3ebd831ac8 100644 --- a/src/main/navigation/navigation_vtol_mc_protection_logic.h +++ b/src/main/navigation/navigation_vtol_mc_protection_logic.h @@ -35,6 +35,16 @@ #define VTOL_MC_FALLBACK_SETTLE_TIME_MS 3000 #define VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS 500 #define VTOL_MC_DEFAULT_BRAKING_DISENGAGE_CM_S 75 +#define VTOL_MC_LANDING_BUMP_SAFE_AGL_CM 50.0f +#define VTOL_MC_LANDING_PROBE_RAMP_TIME_MS 300 +#define VTOL_MC_LANDING_PROBE_CONFIRM_TIME_MS 700 +#define VTOL_MC_LANDING_PROBE_THROTTLE_DROP_PERCENT 10 +#define VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MIN 40 +#define VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MAX 100 +#define VTOL_MC_LANDING_PROBE_AGL_DROP_CM 8.0f +#define VTOL_MC_LANDING_PROBE_VEL_DELTA_CM_S 20.0f +#define VTOL_MC_LANDING_PROBE_VEL_ABS_CM_S 80.0f +#define VTOL_MC_LANDING_PROBE_LOW_G_THRESHOLD 0.85f typedef struct vtolMcProtectionThrottleBounds_s { int16_t min; @@ -226,6 +236,95 @@ static inline bool vtolMcProtectionShouldRelaxAltitude( (captureActive || (velocityUsable && horizontalSpeedCmS > horizontalLimitCmS)); } +static inline bool vtolMcProtectionLandingBumpAllowed( + const bool isVtolMcMode, + const bool navLandingActive, + const bool aglTrusted, + const float aglCm, + const bool verticalVelocityUsable, + const float verticalSpeedCmS, + const float desiredVerticalSpeedCmS, + const float finalDescentDemandLimitCmS, + const float verticalSpeedLimitCmS) +{ + if (!isVtolMcMode) { + return true; + } + + if (aglTrusted) { + return aglCm <= VTOL_MC_LANDING_BUMP_SAFE_AGL_CM; + } + + if (!navLandingActive || !verticalVelocityUsable) { + return false; + } + + const float absVerticalSpeedCmS = verticalSpeedCmS < 0.0f ? -verticalSpeedCmS : verticalSpeedCmS; + return desiredVerticalSpeedCmS > -finalDescentDemandLimitCmS && + absVerticalSpeedCmS <= verticalSpeedLimitCmS; +} + +static inline int16_t vtolMcProtectionLandingProbeThrottleDrop( + const int16_t idleThrottle, + const int16_t hoverThrottle) +{ + const int16_t hoverRange = hoverThrottle > idleThrottle ? hoverThrottle - idleThrottle : 0; + int16_t drop = (hoverRange * VTOL_MC_LANDING_PROBE_THROTTLE_DROP_PERCENT) / 100; + + if (drop < VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MIN) { + drop = VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MIN; + } else if (drop > VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MAX) { + drop = VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MAX; + } + + return drop < hoverRange ? drop : hoverRange; +} + +static inline int16_t vtolMcProtectionLandingProbeThrottle( + const int16_t startThrottle, + const int16_t idleThrottle, + const int16_t hoverThrottle, + const timeMs_t startedAtMs, + const timeMs_t nowMs) +{ + const int16_t safeStartThrottle = startThrottle > idleThrottle ? startThrottle : idleThrottle; + const int16_t throttleDrop = vtolMcProtectionLandingProbeThrottleDrop(idleThrottle, hoverThrottle); + const int16_t targetThrottle = safeStartThrottle > idleThrottle + throttleDrop ? safeStartThrottle - throttleDrop : idleThrottle; + + if (VTOL_MC_LANDING_PROBE_RAMP_TIME_MS == 0) { + return targetThrottle; + } + + const timeMs_t elapsedMs = nowMs - startedAtMs; + if (elapsedMs >= VTOL_MC_LANDING_PROBE_RAMP_TIME_MS) { + return targetThrottle; + } + + return safeStartThrottle - ((safeStartThrottle - targetThrottle) * (int32_t)elapsedMs) / VTOL_MC_LANDING_PROBE_RAMP_TIME_MS; +} + +static inline bool vtolMcProtectionLandingProbeAirborneResponse( + const bool aglTrusted, + const float startAglCm, + const float currentAglCm, + const bool verticalVelocityUsable, + const float startVerticalSpeedCmS, + const float currentVerticalSpeedCmS, + const float accZG) +{ + if (aglTrusted && currentAglCm < startAglCm - VTOL_MC_LANDING_PROBE_AGL_DROP_CM) { + return true; + } + + if (accZG < VTOL_MC_LANDING_PROBE_LOW_G_THRESHOLD) { + return true; + } + + return verticalVelocityUsable && + (currentVerticalSpeedCmS < startVerticalSpeedCmS - VTOL_MC_LANDING_PROBE_VEL_DELTA_CM_S || + currentVerticalSpeedCmS < -VTOL_MC_LANDING_PROBE_VEL_ABS_CM_S); +} + static inline float vtolMcProtectionCommandScaleForSpeed(const float horizontalSpeedCmS) { if (horizontalSpeedCmS <= VTOL_MC_COMMAND_SHAPE_START_CM_S) { diff --git a/src/main/navigation/navigation_vtol_mission_logic.h b/src/main/navigation/navigation_vtol_mission_logic.h new file mode 100644 index 00000000000..ef51285d4d7 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mission_logic.h @@ -0,0 +1,51 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +typedef enum { + NAV_MISSION_VTOL_PRECONDITION_READY = 0, + NAV_MISSION_VTOL_PRECONDITION_WAIT, + NAV_MISSION_VTOL_PRECONDITION_REJECT, +} navMissionVtolTransitionPrecondition_e; + +static inline navMissionVtolTransitionPrecondition_e navMissionVtolTransitionPreconditionDisposition( + const bool armed, + const bool failsafeActive, + const bool sensorsCalibrating, + const bool positionUsable, + const bool headingUsable, + const bool mixerProfileModeConfigured, + const bool hotSwitchAvailable, + const bool mixerAtActive) +{ + if (!armed || failsafeActive) { + return NAV_MISSION_VTOL_PRECONDITION_REJECT; + } + + if (sensorsCalibrating || !positionUsable || !headingUsable || mixerAtActive) { + return NAV_MISSION_VTOL_PRECONDITION_WAIT; + } + + if (!mixerProfileModeConfigured || !hotSwitchAvailable) { + return NAV_MISSION_VTOL_PRECONDITION_REJECT; + } + + return NAV_MISSION_VTOL_PRECONDITION_READY; +} diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 98c3838f687..7d8012de616 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -72,6 +72,7 @@ set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GI set_property(SOURCE mixer_transition_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) set_property(SOURCE mixer_transition_policy_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) set_property(SOURCE mixer_transition_scenarios_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE navigation_vtol_mission_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) set_property(SOURCE vtol_mc_protection_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) function(unit_test src) diff --git a/src/test/unit/navigation_vtol_mission_logic_unittest.cc b/src/test/unit/navigation_vtol_mission_logic_unittest.cc new file mode 100644 index 00000000000..b70966ea6e6 --- /dev/null +++ b/src/test/unit/navigation_vtol_mission_logic_unittest.cc @@ -0,0 +1,113 @@ +#include + +extern "C" { +#include "navigation/navigation_vtol_mission_logic.h" +} + +TEST(NavigationVtolMissionLogicTest, ReadyWhenAllPreconditionsAreMet) +{ + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_READY, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + true, + true, + true, + false)); +} + +TEST(NavigationVtolMissionLogicTest, TemporaryRuntimeConditionsWait) +{ + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_WAIT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + true, + true, + true, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_WAIT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + false, + true, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_WAIT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + false, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_WAIT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + true, + true, + true, + true)); +} + +TEST(NavigationVtolMissionLogicTest, HardSafetyAndConfigurationProblemsReject) +{ + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_REJECT, + navMissionVtolTransitionPreconditionDisposition( + false, + false, + false, + true, + true, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_REJECT, + navMissionVtolTransitionPreconditionDisposition( + true, + true, + false, + true, + true, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_REJECT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + true, + false, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_REJECT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + true, + true, + false, + false)); +} diff --git a/src/test/unit/vtol_mc_protection_logic_unittest.cc b/src/test/unit/vtol_mc_protection_logic_unittest.cc index 10f880b2491..50bb6445b82 100644 --- a/src/test/unit/vtol_mc_protection_logic_unittest.cc +++ b/src/test/unit/vtol_mc_protection_logic_unittest.cc @@ -190,6 +190,183 @@ TEST(VtolMcProtectionLogicTest, SoftAltitudeRelaxationOnlyAppliesDuringCaptureOr true, true, true, true, false, false, false, 75.0f, 75, false)); } +TEST(VtolMcProtectionLogicTest, LandingBumpKeepsLegacyBehaviorForNonVtol) +{ + EXPECT_TRUE(vtolMcProtectionLandingBumpAllowed( + false, + false, + false, + 1000.0f, + false, + -300.0f, + -300.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpAllowsTrustedNearGroundTouchdown) +{ + EXPECT_TRUE(vtolMcProtectionLandingBumpAllowed( + true, + false, + true, + VTOL_MC_LANDING_BUMP_SAFE_AGL_CM, + false, + -300.0f, + -300.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpBlocksTrustedHighAgl) +{ + EXPECT_FALSE(vtolMcProtectionLandingBumpAllowed( + true, + true, + true, + VTOL_MC_LANDING_BUMP_SAFE_AGL_CM + 1.0f, + true, + -20.0f, + -50.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpBlocksVtolHighAltitudeFastDescent) +{ + EXPECT_FALSE(vtolMcProtectionLandingBumpAllowed( + true, + true, + false, + 1000.0f, + true, + -170.0f, + -150.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpAllowsVtolFinalSlowDescent) +{ + EXPECT_TRUE(vtolMcProtectionLandingBumpAllowed( + true, + true, + false, + 1000.0f, + true, + -60.0f, + -50.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpBlocksVtolWithoutTouchdownContext) +{ + EXPECT_FALSE(vtolMcProtectionLandingBumpAllowed( + true, + false, + false, + 1000.0f, + true, + -20.0f, + 0.0f, + 75.0f, + 100.0f)); + + EXPECT_FALSE(vtolMcProtectionLandingBumpAllowed( + true, + true, + false, + 1000.0f, + false, + 0.0f, + -50.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeThrottleDropUsesSmallBoundedReduction) +{ + EXPECT_EQ(40, vtolMcProtectionLandingProbeThrottleDrop(1000, 1200)); + EXPECT_EQ(50, vtolMcProtectionLandingProbeThrottleDrop(1000, 1500)); + EXPECT_EQ(100, vtolMcProtectionLandingProbeThrottleDrop(1000, 2500)); + EXPECT_EQ(0, vtolMcProtectionLandingProbeThrottleDrop(1000, 1000)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeThrottleRampsDownFromStart) +{ + EXPECT_EQ(1500, vtolMcProtectionLandingProbeThrottle(1500, 1000, 1500, 1000, 1000)); + EXPECT_EQ(1475, vtolMcProtectionLandingProbeThrottle(1500, 1000, 1500, 1000, 1150)); + EXPECT_EQ(1450, vtolMcProtectionLandingProbeThrottle(1500, 1000, 1500, 1000, 1300)); + EXPECT_EQ(1000, vtolMcProtectionLandingProbeThrottle(0, 1000, 1500, 1000, 1150)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeDetectsAirborneByAglDrop) +{ + EXPECT_TRUE(vtolMcProtectionLandingProbeAirborneResponse( + true, + 40.0f, + 30.0f, + false, + 0.0f, + 0.0f, + 1.0f)); + + EXPECT_FALSE(vtolMcProtectionLandingProbeAirborneResponse( + true, + 40.0f, + 35.0f, + false, + 0.0f, + 0.0f, + 1.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeDetectsAirborneByLowG) +{ + EXPECT_TRUE(vtolMcProtectionLandingProbeAirborneResponse( + false, + 0.0f, + 0.0f, + false, + 0.0f, + 0.0f, + VTOL_MC_LANDING_PROBE_LOW_G_THRESHOLD - 0.01f)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeDetectsAirborneByVerticalVelocityChange) +{ + EXPECT_TRUE(vtolMcProtectionLandingProbeAirborneResponse( + false, + 0.0f, + 0.0f, + true, + -20.0f, + -45.0f, + 1.0f)); + + EXPECT_TRUE(vtolMcProtectionLandingProbeAirborneResponse( + false, + 0.0f, + 0.0f, + true, + -50.0f, + -90.0f, + 1.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeDoesNotReactWithoutDescentEvidence) +{ + EXPECT_FALSE(vtolMcProtectionLandingProbeAirborneResponse( + false, + 0.0f, + 0.0f, + true, + -20.0f, + -30.0f, + 1.0f)); +} + TEST(VtolMcProtectionLogicTest, BailoutAngleLimitUsesBankAngleWithSafeClamps) { EXPECT_EQ(450, vtolMcProtectionBailoutAngleLimitDeciDeg(20)); From 3699c17a39d943eecc41e700497b6ef53cbd6f01 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Wed, 17 Jun 2026 18:16:42 +0300 Subject: [PATCH 36/42] Fix VTOL transition failsafe handling and F7 ITCM usage Allow navigation-owned RTH/LAND VTOL transitions to continue during failsafe while still aborting unsafe manual or mission transition requests. Keep the post-switch output ramp active after the target profile is selected so failsafe does not cause an abrupt transition output reset. Move auto-transition target PID and motor-mix preparation out of F7 FAST_CODE hot paths, prevent LTO from inlining transition helpers into ITCM, and avoid repeated pusher-scale reads inside the motor loop. This fixes the OMNIBUSF7/OMNIBUSF7V2 16 KB ITCM build pressure without changing transition behavior. Add policy tests for failsafe-allowed transition requests and post-switch ramp continuation. --- src/main/flight/mixer.c | 137 ++++++++++-------- src/main/flight/mixer_profile.c | 24 +-- src/main/flight/mixer_transition_logic.h | 26 ++++ src/main/flight/pid.c | 6 +- src/main/navigation/navigation.c | 16 +- .../unit/mixer_transition_policy_unittest.cc | 25 ++++ 6 files changed, 158 insertions(+), 76 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 3f5b317d4d2..4da18bc8ccf 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -265,6 +265,53 @@ void mixerResetDisarmedMotors(void) motor_disarmed[i] = motorZeroCommand; } } + +#ifdef USE_AUTO_TRANSITION +typedef struct autoTransitionMotorMixState_s { + bool active; + bool currentProfileIsMultirotor; + bool targetProfileIsMultirotor; + const motorMixer_t *targetMotorMixer; + const mixerConfig_t *targetMixerConfig; + int16_t targetInput[3]; +} autoTransitionMotorMixState_t; + +static void NOINLINE prepareAutoTransitionMotorMixState(autoTransitionMotorMixState_t *state) +{ + if (!(isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction != MIXERAT_DIRECTION_NONE && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT)) { + return; + } + + state->active = true; + state->currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); + state->targetMotorMixer = mixerMotorMixersByIndex(nextMixerProfileIndex); + state->targetMixerConfig = mixerConfigByIndex(nextMixerProfileIndex); + state->targetProfileIsMultirotor = isMultirotorTypePlatform(state->targetMixerConfig->platformType); + + const float targetAuthorityScale = state->targetProfileIsMultirotor ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + + if (!state->targetProfileIsMultirotor && FLIGHT_MODE(MANUAL_MODE)) { + state->targetInput[ROLL] = rcCommand[ROLL]; + state->targetInput[PITCH] = rcCommand[PITCH]; + state->targetInput[YAW] = rcCommand[YAW]; + } else { + state->targetInput[ROLL] = getAutoTransitionTargetAxisPID(FD_ROLL); + state->targetInput[PITCH] = getAutoTransitionTargetAxisPID(FD_PITCH); + state->targetInput[YAW] = getAutoTransitionTargetAxisPID(FD_YAW); + } + + state->targetInput[ROLL] = state->targetInput[ROLL] * (state->targetMixerConfig->transition_PID_mmix_multiplier_roll / 1000.0f) * targetAuthorityScale; + state->targetInput[PITCH] = state->targetInput[PITCH] * (state->targetMixerConfig->transition_PID_mmix_multiplier_pitch / 1000.0f) * targetAuthorityScale; + state->targetInput[YAW] = state->targetInput[YAW] * (state->targetMixerConfig->transition_PID_mmix_multiplier_yaw / 1000.0f) * targetAuthorityScale; +} +#endif + #if !defined(SITL_BUILD) static uint16_t handleOutputScaling( int16_t input, // Input value from the mixer @@ -538,38 +585,10 @@ void FAST_CODE mixTable(void) } #ifdef USE_AUTO_TRANSITION - const bool autoTransitionMotorMixing = isMixerTransitionMixing && - mixerATIsActive() && - mixerProfileAT.direction != MIXERAT_DIRECTION_NONE && - nextMixerProfileIndex >= 0 && - nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT; - const bool currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); - const int targetMixerProfileIndex = autoTransitionMotorMixing ? nextMixerProfileIndex : -1; - const motorMixer_t *targetMotorMixer = autoTransitionMotorMixing ? mixerMotorMixersByIndex(targetMixerProfileIndex) : NULL; - const mixerConfig_t *targetMixerConfig = autoTransitionMotorMixing ? mixerConfigByIndex(targetMixerProfileIndex) : NULL; - const bool targetProfileIsMultirotor = autoTransitionMotorMixing ? - isMultirotorTypePlatform(targetMixerConfig->platformType) : - false; - int16_t targetInput[3] = { 0, 0, 0 }; - - if (autoTransitionMotorMixing) { - const float targetAuthorityScale = targetProfileIsMultirotor ? - mixerATGetMcAuthorityScale() : - mixerATGetFwAuthorityScale(); - - if (!targetProfileIsMultirotor && FLIGHT_MODE(MANUAL_MODE)) { - targetInput[ROLL] = rcCommand[ROLL]; - targetInput[PITCH] = rcCommand[PITCH]; - targetInput[YAW] = rcCommand[YAW]; - } else { - targetInput[ROLL] = getAutoTransitionTargetAxisPID(FD_ROLL); - targetInput[PITCH] = getAutoTransitionTargetAxisPID(FD_PITCH); - targetInput[YAW] = getAutoTransitionTargetAxisPID(FD_YAW); - } + autoTransitionMotorMixState_t autoTransition = { 0 }; - targetInput[ROLL] = targetInput[ROLL] * (targetMixerConfig->transition_PID_mmix_multiplier_roll / 1000.0f) * targetAuthorityScale; - targetInput[PITCH] = targetInput[PITCH] * (targetMixerConfig->transition_PID_mmix_multiplier_pitch / 1000.0f) * targetAuthorityScale; - targetInput[YAW] = targetInput[YAW] * (targetMixerConfig->transition_PID_mmix_multiplier_yaw / 1000.0f) * targetAuthorityScale; + if (isMixerTransitionMixing) { + prepareAutoTransitionMotorMixState(&autoTransition); } #endif @@ -581,7 +600,7 @@ void FAST_CODE mixTable(void) // motors for non-servo mixes for (int i = 0; i < motorCount; i++) { #ifdef USE_AUTO_TRANSITION - const motorMixer_t *targetMixer = autoTransitionMotorMixing ? &targetMotorMixer[i] : NULL; + const motorMixer_t *targetMixer = autoTransition.active ? &autoTransition.targetMotorMixer[i] : NULL; const bool currentMotorActive = currentMixer[i].throttle > 0.0f; const bool targetMotorActive = targetMixer && targetMixer->throttle > 0.0f; const float activeRpyMix = currentMotorActive ? @@ -590,17 +609,17 @@ void FAST_CODE mixTable(void) -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) : 0; const float targetRpyMix = targetMotorActive ? - (targetInput[PITCH] * targetMixer->pitch + - targetInput[ROLL] * targetMixer->roll + - -motorYawMultiplier * targetInput[YAW] * targetMixer->yaw) : + (autoTransition.targetInput[PITCH] * targetMixer->pitch + + autoTransition.targetInput[ROLL] * targetMixer->roll + + -motorYawMultiplier * autoTransition.targetInput[YAW] * targetMixer->yaw) : 0; float sharedRpyNormalizer = 1.0f; if (currentMotorActive && targetMotorActive) { - const float activeAuthorityScale = currentProfileIsMultirotor ? + const float activeAuthorityScale = autoTransition.currentProfileIsMultirotor ? mixerATGetMcAuthorityScale() : mixerATGetFwAuthorityScale(); - const float targetAuthorityScale = targetProfileIsMultirotor ? + const float targetAuthorityScale = autoTransition.targetProfileIsMultirotor ? mixerATGetMcAuthorityScale() : mixerATGetFwAuthorityScale(); const float authorityScaleSum = activeAuthorityScale + targetAuthorityScale; @@ -703,34 +722,36 @@ void FAST_CODE mixTable(void) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. +#ifdef USE_AUTO_TRANSITION + const float transitionPusherScale = isMixerTransitionMixing ? mixerATGetPusherScale() : 1.0f; +#endif for (int i = 0; i < motorCount; i++) { float motorThrottle = mixerThrottleCommand * currentMixer[i].throttle; #ifdef USE_AUTO_TRANSITION - const motorMixer_t *targetMixer = autoTransitionMotorMixing ? &targetMotorMixer[i] : NULL; + const motorMixer_t *targetMixer = autoTransition.active ? &autoTransition.targetMotorMixer[i] : NULL; const bool currentMotorActive = currentMixer[i].throttle > 0.0f; const bool targetMotorActive = targetMixer && targetMixer->throttle > 0.0f; const bool sharedMotor = currentMotorActive && targetMotorActive; const bool currentMotorPlaceholder = currentMixer[i].throttle <= 0.0f && currentMixer[i].throttle > -1.05f; - const float pusherScale = mixerATGetPusherScale(); - if (autoTransitionMotorMixing && currentMotorActive) { - if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && currentProfileIsMultirotor) { - if (sharedMotor && !targetProfileIsMultirotor) { - const float propulsionBlend = pusherScale; + if (autoTransition.active && currentMotorActive) { + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && autoTransition.currentProfileIsMultirotor) { + if (sharedMotor && !autoTransition.targetProfileIsMultirotor) { + const float propulsionBlend = transitionPusherScale; const float blendedThrottle = currentMixer[i].throttle * (1.0f - propulsionBlend) + targetMixer->throttle * propulsionBlend; motorThrottle = mixerThrottleCommand * blendedThrottle; } else { motorThrottle *= mixerATGetLiftScale(); } - } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && !currentProfileIsMultirotor) { - if (sharedMotor && targetProfileIsMultirotor) { - const float mcBlend = 1.0f - pusherScale; - const float blendedThrottle = currentMixer[i].throttle * pusherScale + + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && !autoTransition.currentProfileIsMultirotor) { + if (sharedMotor && autoTransition.targetProfileIsMultirotor) { + const float mcBlend = 1.0f - transitionPusherScale; + const float blendedThrottle = currentMixer[i].throttle * transitionPusherScale + targetMixer->throttle * mcBlend; motorThrottle = mixerThrottleCommand * blendedThrottle; } else { - motorThrottle *= pusherScale; + motorThrottle *= transitionPusherScale; } } } @@ -749,22 +770,22 @@ void FAST_CODE mixTable(void) motor[i] = motorZeroCommand; } #ifdef USE_AUTO_TRANSITION - if (autoTransitionMotorMixing && + if (autoTransition.active && mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && - currentProfileIsMultirotor && + autoTransition.currentProfileIsMultirotor && currentMotorPlaceholder && targetMotorActive && - !targetProfileIsMultirotor) { + !autoTransition.targetProfileIsMultirotor) { // Smooth auto-transition pusher preview comes from the target FW // mixer rule. A zero current rule reserves the physical motor index. - const float targetMotorThrottle = mixerThrottleCommand * targetMixer->throttle * pusherScale; + const float targetMotorThrottle = mixerThrottleCommand * targetMixer->throttle * transitionPusherScale; motor[i] = constrain(rpyMix[i] + constrain(targetMotorThrottle, throttleMin, throttleMax), throttleRangeMin, throttleRangeMax); - } else if (autoTransitionMotorMixing && + } else if (autoTransition.active && mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && - !currentProfileIsMultirotor && + !autoTransition.currentProfileIsMultirotor && currentMotorPlaceholder && targetMotorActive && - targetProfileIsMultirotor) { + autoTransition.targetProfileIsMultirotor) { // Target MC lift preview uses the target MC mixer rule and target // MC PID preview. This avoids feeding active FW/PIFF PID into lift // motors while still allowing MC control to fade in before switch. @@ -775,10 +796,9 @@ void FAST_CODE mixTable(void) //spin stopped motors only in mixer transition mode if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && !feature(FEATURE_REVERSIBLE_MOTORS)) { #ifdef USE_AUTO_TRANSITION - const float pusherScale = mixerATGetPusherScale(); const float pusherTarget = -currentMixer[i].throttle * 1000.0f; const float pusherIdle = throttleRangeMin; - motor[i] = pusherIdle + (pusherTarget - pusherIdle) * pusherScale; + motor[i] = pusherIdle + (pusherTarget - pusherIdle) * transitionPusherScale; #else motor[i] = -currentMixer[i].throttle * 1000; #endif @@ -786,7 +806,8 @@ void FAST_CODE mixTable(void) } #ifdef USE_AUTO_TRANSITION int16_t postSwitchFadeOutput = 0; - if (mixerATGetPostSwitchFadeMotorOutput(i, motorZeroCommand, motor[i], &postSwitchFadeOutput)) { + if (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE && + mixerATGetPostSwitchFadeMotorOutput(i, motorZeroCommand, motor[i], &postSwitchFadeOutput)) { motor[i] = constrain(postSwitchFadeOutput, motorZeroCommand, getMaxThrottle()); } #endif diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index eaf731afd42..7a8346b3f1e 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -955,7 +955,13 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) manualFwToMcProtectionLatched = false; } - if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { + const bool failsafeShouldAbortTransition = FLIGHT_MODE(FAILSAFE_MODE) && + mixerTransitionShouldAbortForFailsafe( + mixerProfileAT.request, + mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE, + mixerProfileAT.hotSwitchDone); + + if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || failsafeShouldAbortTransition || areSensorsCalibrating())) { abortTransition(false, false); manualTransitionSessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; manualFwToMcProtectionLatched = false; @@ -1125,7 +1131,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) #endif } -bool mixerATIsActive(void) +bool NOINLINE mixerATIsActive(void) { return mixerProfileAT.phase != MIXERAT_PHASE_IDLE; } @@ -1148,7 +1154,7 @@ bool mixerATWasAbortedByAirspeedTimeout(void) #endif } -float mixerATGetPusherScale(void) +float NOINLINE mixerATGetPusherScale(void) { #ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f); @@ -1157,7 +1163,7 @@ float mixerATGetPusherScale(void) #endif } -float mixerATGetLiftScale(void) +float NOINLINE mixerATGetLiftScale(void) { #ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f); @@ -1166,7 +1172,7 @@ float mixerATGetLiftScale(void) #endif } -float mixerATGetMcAuthorityScale(void) +float NOINLINE mixerATGetMcAuthorityScale(void) { #ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f); @@ -1175,7 +1181,7 @@ float mixerATGetMcAuthorityScale(void) #endif } -float mixerATGetFwAuthorityScale(void) +float NOINLINE mixerATGetFwAuthorityScale(void) { #ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); @@ -1184,7 +1190,7 @@ float mixerATGetFwAuthorityScale(void) #endif } -float mixerATGetBlendToFw(void) +float NOINLINE mixerATGetBlendToFw(void) { #ifdef USE_AUTO_TRANSITION return constrainf(mixerProfileAT.blendToFw, 0.0f, 1.0f); @@ -1193,7 +1199,7 @@ float mixerATGetBlendToFw(void) #endif } -int16_t mixerATGetTransitionServoInput(void) +int16_t NOINLINE mixerATGetTransitionServoInput(void) { #ifdef USE_AUTO_TRANSITION const bool postSwitchFadeToFwActive = @@ -1268,7 +1274,7 @@ bool mixerATGetServoHandoffOutput(uint8_t servoIndex, int16_t currentOutput, int return true; } -bool mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output) +bool NOINLINE mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output) { if (mixerProfileAT.phase != MIXERAT_PHASE_POST_SWITCH_FADE || motorIndex >= MAX_SUPPORTED_MOTORS || diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h index af0ec5e7b70..c1df44cb145 100644 --- a/src/main/flight/mixer_transition_logic.h +++ b/src/main/flight/mixer_transition_logic.h @@ -110,6 +110,32 @@ static inline bool mixerTransitionIsRequestAllowed( } } +static inline bool mixerTransitionRequestAllowedDuringFailsafe(mixerProfileATRequest_e requiredAction) +{ + switch (requiredAction) { + case MIXERAT_REQUEST_RTH: + case MIXERAT_REQUEST_LAND: + return true; + + default: + return false; + } +} + +static inline bool mixerTransitionShouldAbortForFailsafe( + const mixerProfileATRequest_e requiredAction, + const bool postSwitchActive, + const bool hotSwitchDone) +{ + if (mixerTransitionRequestAllowedDuringFailsafe(requiredAction)) { + return false; + } + + // Once the target profile is already active, finishing the remaining output + // ramp is safer than cancelling into an abrupt scale reset. + return !(postSwitchActive && hotSwitchDone); +} + #ifdef USE_AUTO_TRANSITION static inline float mixerTransitionComputeMotorRampProgress( bool dynamicMixerEnabled, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8de52eb6e52..57755b1928b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -492,7 +492,7 @@ float getAxisIterm(uint8_t axis) } #ifdef USE_AUTO_TRANSITION -static void resetAutoTransitionTargetPidState(void) +static void NOINLINE resetAutoTransitionTargetPidState(void) { memset(autoTransitionTargetPidState, 0, sizeof(autoTransitionTargetPidState)); memset(autoTransitionTargetAxisPID, 0, sizeof(autoTransitionTargetAxisPID)); @@ -1155,7 +1155,7 @@ static int16_t applyAutoTransitionTargetMulticopterRateController(autoTransition return newOutputLimited; } -static void updateAutoTransitionTargetAxisPID(float dT) +static void NOINLINE updateAutoTransitionTargetAxisPID(float dT) { if (!pidFiltersConfigured || !isAutoTransitionTargetPidActive()) { resetAutoTransitionTargetPidState(); @@ -1194,7 +1194,7 @@ static void updateAutoTransitionTargetAxisPID(float dT) } } -int16_t getAutoTransitionTargetAxisPID(flight_dynamics_index_t axis) +int16_t NOINLINE getAutoTransitionTargetAxisPID(flight_dynamics_index_t axis) { if (!isAutoTransitionTargetPidActive()) { return 0; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f3446e28fd7..ea5fca6caa2 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -48,6 +48,7 @@ #include "flight/imu.h" #include "flight/mixer_profile.h" +#include "flight/mixer_transition_logic.h" #include "flight/pid.h" #include "flight/wind_estimator.h" @@ -1949,6 +1950,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF } if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){ + // Fixed-wing VTOL can only pass through the landing area; switch to MC + // before applying the MC-only settle gate below. return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } @@ -2842,12 +2845,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav UNUSED(previousState); #ifdef USE_AUTO_TRANSITION - if (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE)) { - mixerATUpdateState(MIXERAT_REQUEST_ABORT); - clearMissionVTOLTransitionState(); - return NAV_FSM_EVENT_SWITCH_TO_IDLE; - } - mixerProfileATRequest_e required_action; switch (navMixerATPendingState) { @@ -2865,6 +2862,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav break; } + if (!ARMING_FLAG(ARMED) || + (FLIGHT_MODE(FAILSAFE_MODE) && mixerTransitionShouldAbortForFailsafe(required_action, false, false))) { + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + clearMissionVTOLTransitionState(); + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + if (navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE) { const navMixerATRetryScanResult_e retryResult = updateMissionTransitionRetryScan(); if (retryResult == NAV_MIXERAT_RETRY_SCAN_READY_TO_RETRY) { diff --git a/src/test/unit/mixer_transition_policy_unittest.cc b/src/test/unit/mixer_transition_policy_unittest.cc index 4ff79f325b9..5894d25f6f3 100644 --- a/src/test/unit/mixer_transition_policy_unittest.cc +++ b/src/test/unit/mixer_transition_policy_unittest.cc @@ -160,6 +160,31 @@ TEST(MixerTransitionPolicyTest, ManualRequestsNeedMixerProfileModeAndMatchingTar true)); } +TEST(MixerTransitionPolicyTest, OnlyNavigationOwnedRthAndLandRequestsMayContinueDuringFailsafe) +{ + EXPECT_TRUE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_RTH)); + EXPECT_TRUE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_LAND)); + + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_NONE)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MISSION_TO_FW)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MISSION_TO_MC)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MANUAL_TO_FW)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MANUAL_TO_MC)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_ABORT)); +} + +TEST(MixerTransitionPolicyTest, FailsafeKeepsPostSwitchOutputRampAfterRequestIsCleared) +{ + EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_RTH, false, false)); + EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_LAND, false, false)); + + EXPECT_TRUE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, false, false)); + EXPECT_TRUE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, true, false)); + EXPECT_TRUE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, false, true)); + + EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, true, true)); +} + TEST(MixerTransitionPolicyTest, DynamicScalingDisabledKeepsAllScalesAtFullValues) { const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( From 0bab61f8784ba755a4b7781ce3d9acacbb3f4f62 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 18 Jun 2026 09:50:05 +0300 Subject: [PATCH 37/42] Add VTOL transition OSD status and low-speed MC fallback Add shared OSD messages for VTOL auto-transition phases, including manual transitions, mission transitions, RTH/landing transitions, retry scan/align states, aborts, airspeed timeouts, completion, and low-speed FW-to-MC safety fallback. Extend vtol_fw_to_mc_auto_switch_airspeed_cm_s from manual-only protection to a navigation-owned safety fallback for mission, RTH, and failsafe RTH. When trusted pitot airspeed drops below the configured threshold in FW mode, navigation can request a FW-to-MC transition if mixer_automated_switch is enabled and a valid MC target profile exists. Keep the current NAV session in MC after the low-speed fallback to avoid repeated MC->FW->MC loops. Resume mission and RTH flow after the fallback, including waypoint enroute/hold and RTH near-home states, while ignoring later mission USER=FW requests until navigation is restarted. Document the updated VTOL transition, RTH/failsafe, stabilization, and landing-detection behavior, and add policy test coverage for request gating, trusted-low-airspeed triggering, and failsafe handling. --- docs/MixerProfile.md | 9 +- docs/Navigation.md | 12 ++ docs/Settings.md | 2 +- docs/VTOL.md | 31 ++- src/main/fc/settings.yaml | 2 +- src/main/flight/mixer_profile.c | 50 ++++- src/main/flight/mixer_profile.h | 20 ++ src/main/flight/mixer_transition_logic.h | 22 ++ src/main/io/osd.c | 5 + src/main/io/osd.h | 16 ++ src/main/io/osd_common.c | 101 +++++++++ src/main/io/osd_common.h | 1 + src/main/io/osd_dji_hd.c | 47 +++-- src/main/navigation/navigation.c | 198 +++++++++++++++--- src/main/navigation/navigation.h | 11 + .../unit/mixer_transition_policy_unittest.cc | 45 +++- 16 files changed, 514 insertions(+), 58 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 8cac8b1b812..ff5179f767b 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -39,7 +39,7 @@ If `mixer_vtol_manualswitch_autotransition_controller = ON`, `MIXER TRANSITION` - Leaving the switch ON does not keep restarting the transition. - To start another transition, turn the switch OFF and then ON again. - If you turn the switch OFF before the profile change happens, that transition request is cancelled. -- Optional extra protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can automatically start FW->MC if airspeed gets too low. After that switch, INAV stays in MC until you deliberately command another manual profile change. +- Optional extra protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can automatically start FW->MC if trusted pitot airspeed gets too low. During manual FW flight, INAV stays in MC until you deliberately command another manual profile change. During mission/RTH/failsafe, INAV keeps the current navigation task in MC after the safety switch. This behavior is controlled by `mixer_vtol_manualswitch_autotransition_controller`. Turn it ON in both mixer profiles if you want the same switch behavior in both directions. @@ -49,6 +49,9 @@ In Active Modes: - `MIXER TRANSITION` shows that the internal transition logic is actually running. - `MIXER PROFILE 2` shows that mixer profile 2 is currently active. +- During a smooth auto-transition, `MIXER TRANSITION` may remain active while INAV is still using the source profile, waiting for airspeed/timer completion, or finishing the safe output movement after the profile switch. +- During the same transition, `MIXER PROFILE 2` changes only when the actual active mixer profile changes. It is not a progress indicator. +- Mission, RTH, and LAND auto-transitions use the same internal transition state, so `MIXER TRANSITION` can appear active even when the RC `MIXER TRANSITION` switch was not used. Use the OSD system message or `DEBUG_VTOL_TRANSITION` when you need to see which internal transition state is active. There are two separate manual paths: @@ -64,7 +67,7 @@ There are two separate manual paths: - Profile 2 = MC - One supported mapping is: - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) - - Pos2 = Transition request (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) + - Pos2 = Transition request (`MIXER PROFILE 2` ON, `MIXER TRANSITION` ON) - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) - Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. - If you intentionally swap the profile order, keep the same idea and swap the FW and MC end positions. @@ -147,6 +150,8 @@ This section applies only to targets with more than 512 KB flash, compiled with Direct manual `MIXER PROFILE 2` switching is still a separate path when you want an immediate profile change. +The OSD system message field reports the internal transition state for manual, mission, RTH, LAND, retry, abort, timeout, and completion events. This is useful because Active Modes show RC mode/profile state, while the OSD message describes what the auto-transition controller is doing. + ### Airspeed-first completion When valid pitot airspeed is available, INAV uses airspeed to decide when the transition is complete: diff --git a/docs/Navigation.md b/docs/Navigation.md index 3321cced707..35135e744a1 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -136,12 +136,24 @@ Transition behavior in this MVP: - Ground speed is not used for transition progress/completion. - 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. +- If an airspeed-controlled MC -> FW transition times out, `nav_vtol_transition_retry_on_airspeed_timeout` can run one heading scan/retry before the configured fail action is used. Safety and scope: - This path uses authorized automated transition state handling; it does not permit manual mixer profile switching during normal waypoint navigation. - It still depends on valid mixer profile switching infrastructure (two configured mixer profiles and a valid `MIXER PROFILE 2` mode activation condition). +RTH and failsafe VTOL transitions: + +- RTH may request MC -> FW before flying home if the aircraft is in MC and far enough from home. +- RTH landing may request FW -> MC before using the MC landing controller. +- Failsafe RTH/LAND is allowed to continue those navigation-owned `RTH` and `LAND` transition requests. +- `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can also request a navigation-owned FW -> MC safety transition during mission, RTH, or failsafe RTH when trusted pitot airspeed falls too low. +- This low-speed safety transition requires `mixer_automated_switch = ON` and a valid MC target profile. +- After the low-speed safety transition switches to MC, INAV keeps the current navigation task in MC and blocks automatic MC -> FW RTH or mission re-entry for that navigation session. +- Manual and mission transition requests are not allowed to continue just because failsafe became active; they are aborted unless the target profile has already been selected and INAV is only finishing the remaining safe output movement. +- `vtol_transition_to_mc_max_airspeed_cm_s` controls when an already-requested FW -> MC transition is considered safe to complete. + ### VTOL MC navigation protection and landing detection Targets with more than 512 KB flash can enable extra protection for VTOL aircraft flying in MC mode: diff --git a/docs/Settings.md b/docs/Settings.md index 8ca6a1fcfff..364f7c713de 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -7101,7 +7101,7 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, ### vtol_fw_to_mc_auto_switch_airspeed_cm_s -Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, INAV automatically starts FW->MC. After the switch to MC, INAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. Available only on targets with more than 512 KB flash. +Extra low-speed fixed-wing safety fallback [cm/s]. If trusted pitot airspeed falls to this value or lower while in FW, INAV automatically starts FW->MC. In manual FW flight this requires `mixer_vtol_manualswitch_autotransition_controller` to be ON and keeps MC until the pilot deliberately commands another manual profile change. In mission/RTH/failsafe this requires `mixer_automated_switch` to be ON and keeps the current navigation task in MC after the safety switch, blocking automatic MC->FW re-entry for that navigation session. `vtol_transition_to_mc_max_airspeed_cm_s` still controls when the FW->MC transition is safe to complete. Set to 0 to disable. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md index f4f750d5659..3c92b7b9783 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -387,7 +387,16 @@ Optional low-speed protection: set vtol_fw_to_mc_auto_switch_airspeed_cm_s = 750 ``` -With this set, fixed-wing flight automatically starts FW -> MC when trusted pitot airspeed drops to `7.5 m/s` or lower. After this protection switches to MC, INAV stays in MC until you deliberately command another manual profile change. Set it to `0` to disable this protection. +With this set, fixed-wing flight automatically starts FW -> MC when trusted pitot airspeed drops to `7.5 m/s` or lower. Set it to `0` to disable this protection. + +This is a safety fallback for cases where fixed-wing flight no longer looks safe, for example a pusher problem or another failure that prevents the aircraft from keeping enough airspeed. + +It can be used in two contexts: + +- During manual FW flight, it requires `mixer_vtol_manualswitch_autotransition_controller = ON`. After the switch, INAV stays in MC until you deliberately command another manual profile change. +- During mission, RTH, and failsafe RTH, it is a navigation-owned safety transition. It requires `mixer_automated_switch = ON` and a valid MC target profile. After it switches to MC, INAV keeps the current navigation task in MC and does not automatically start another MC -> FW transition for that navigation session, even if a later mission waypoint requests FW. + +This setting decides when the emergency FW -> MC fallback starts. `vtol_transition_to_mc_max_airspeed_cm_s` still decides when the FW -> MC profile switch is safe to complete during an airspeed-controlled FW -> MC transition. ## 2. Manual switch auto transition with dynamic scaling @@ -970,6 +979,24 @@ Useful debug modes: - `debug[6]`: max absolute roll/pitch attitude [deci-degrees]. - `debug[7]`: capture/landing/bailout settle elapsed time [ms], or command scale x1000 when command shaping is active, otherwise `1000`. +OSD system messages: + +INAV shows VTOL transition progress in the normal OSD system message field, including analog/MSP DisplayPort OSD and DJI HD OSD message output. + +Typical messages include: + +- `VTOL MANUAL TO FW` or `VTOL MANUAL TO MC`: manual switch transition is running. +- `VTOL MISSION TO FW` or `VTOL MISSION TO MC`: waypoint USER action requested a mission transition. +- `VTOL RTH TO FW`: RTH requested MC -> FW before flying home. +- `VTOL LAND TO MC`: RTH or mission landing requested FW -> MC before MC landing. +- `VTOL LOW SPD TO MC`: low-speed protection requested FW -> MC as a safety fallback. +- `VTOL RETRY SCAN`: mission/RTH MC -> FW timed out on airspeed and INAV is scanning headings for a better airspeed direction. +- `VTOL RETRY ALIGN`: INAV is turning to the best heading found by the retry scan before trying again. +- `VTOL FINISHING SWITCH`: the target profile is already active and old propulsion output is being moved to its safe final value. +- `VTOL AIRSPEED TIMEOUT`: the requested pitot airspeed was not reached before `mixer_vtol_transition_airspeed_timeout_ms`. +- `VTOL TRANSITION ABORTED`: the transition was cancelled before the profile switch. +- `VTOL TRANSITION DONE`: the transition finished. + Per-mixer-profile settings: - `mixer_automated_switch`: existing RTH use is to allow NAV to switch between MC and FW for return/landing. New mission use is to allow waypoint USER actions to request MC/FW transitions. @@ -983,7 +1010,7 @@ Global VTOL transition settings: - `vtol_transition_to_fw_min_airspeed_cm_s`: new preferred MC -> FW completion threshold when pitot is trusted. `0` uses the timer path. - `vtol_transition_to_mc_max_airspeed_cm_s`: new preferred FW -> MC completion threshold when pitot is trusted. `0` uses the timer path. -- `vtol_fw_to_mc_auto_switch_airspeed_cm_s`: new low-speed fixed-wing protection. When non-zero, FW can automatically start FW -> MC if pitot airspeed falls too low. +- `vtol_fw_to_mc_auto_switch_airspeed_cm_s`: new low-speed fixed-wing safety fallback. When non-zero, FW flight can automatically start FW -> MC if trusted pitot airspeed falls too low. In manual FW flight it requires the manual auto-transition controller. In mission/RTH/failsafe it requires `mixer_automated_switch = ON` and keeps the current navigation task in MC after the switch. - `vtol_transition_lift_min_percent`: new lowest lift motor power during dynamic transition scaling. `100` keeps full lift power. - `vtol_transition_mc_authority_min_percent`: new lowest MC motor stabilisation strength during dynamic transition scaling. `100` keeps full MC stabilisation. - `vtol_transition_fw_authority_min_percent`: new lowest FW control strength during dynamic transition scaling. It also scales optional target fixed-wing servo preview rules. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0773d3a22c6..4984f89115f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4048,7 +4048,7 @@ groups: min: 0 max: 20000 - name: vtol_fw_to_mc_auto_switch_airspeed_cm_s - description: "Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, INAV automatically starts FW->MC. After the switch to MC, INAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. Available only on targets with more than 512 KB flash." + description: "Extra low-speed fixed-wing safety fallback [cm/s]. If trusted pitot airspeed falls to this value or lower while in FW, INAV automatically starts FW->MC. In manual FW flight this requires `mixer_vtol_manualswitch_autotransition_controller` to be ON and keeps MC until the pilot deliberately commands another manual profile change. In mission/RTH/failsafe this requires `mixer_automated_switch` to be ON and keeps the current navigation task in MC after the safety switch, blocking automatic MC->FW re-entry for that navigation session. `vtol_transition_to_mc_max_airspeed_cm_s` still controls when the FW->MC transition is safe to complete. Set to 0 to disable. Available only on targets with more than 512 KB flash." condition: USE_AUTO_TRANSITION default_value: 0 field: vtolFwToMcAutoSwitchAirspeed diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 7a8346b3f1e..14f74128915 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -11,6 +11,7 @@ #include "drivers/pwm_mapping.h" #include "drivers/time.h" #include "common/maths.h" +#include "common/utils.h" #include "flight/mixer.h" #include "flight/mixer_profile.h" #include "flight/mixer_transition_logic.h" @@ -38,6 +39,7 @@ #ifdef USE_AUTO_TRANSITION #define MIXER_TRANSITION_DIRECT_SWITCH_SERVO_HOLD_MS 300 +#define MIXER_TRANSITION_OSD_EVENT_DISPLAY_MS 3000 #endif mixerConfig_t currentMixerConfig; @@ -52,6 +54,8 @@ static bool manualTransitionReadyForEdge = true; static mixerTransitionManualSessionMode_e manualTransitionSessionMode; static bool manualFwToMcProtectionLatched; static int16_t mixerTransitionServoInput; +static mixerProfileATOsdEvent_e mixerATOsdEvent; +static timeMs_t mixerATOsdEventUntil; #endif // Keep PG version split because USE_AUTO_TRANSITION changes the stored mixer profile layout only on >512 KB targets. @@ -177,6 +181,12 @@ void setMixerProfileAT(void) } #ifdef USE_AUTO_TRANSITION +static void setMixerATOsdEvent(const mixerProfileATOsdEvent_e event) +{ + mixerATOsdEvent = event; + mixerATOsdEventUntil = event == MIXERAT_OSD_EVENT_NONE ? 0 : millis() + MIXER_TRANSITION_OSD_EVENT_DISPLAY_MS; +} + static void clearServoHandoffFade(void) { mixerProfileAT.servoHandoffMask = 0; @@ -392,7 +402,8 @@ static mixerProfileATDirection_e directionForRequest(const mixerProfileATRequest if (required_action == MIXERAT_REQUEST_LAND || required_action == MIXERAT_REQUEST_MISSION_TO_MC || - required_action == MIXERAT_REQUEST_MANUAL_TO_MC) { + required_action == MIXERAT_REQUEST_MANUAL_TO_MC || + required_action == MIXERAT_REQUEST_FW_TO_MC_PROTECTION) { return MIXERAT_DIRECTION_TO_MC; } @@ -601,6 +612,7 @@ static bool updatePostSwitchFade(void) return false; } + setMixerATOsdEvent(MIXERAT_OSD_EVENT_DONE); mixerProfileAT.phase = MIXERAT_PHASE_IDLE; mixerProfileAT.request = MIXERAT_REQUEST_NONE; mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; @@ -638,7 +650,7 @@ static bool shouldRequestManualFwToMcProtection(const bool manualControllerEnabl return false; } - return airspeedCmS <= thresholdCmS; + return mixerTransitionFwToMcProtectionTriggered(STATE(AIRPLANE), thresholdCmS, true, airspeedCmS); } static void updateTransitionScales(void) @@ -671,6 +683,10 @@ static void abortTransition(const bool byAirspeedTimeout, const bool holdServoOu prepareServoHandoffFade(servoHandoffMask); } + if (wasActive) { + setMixerATOsdEvent(byAirspeedTimeout ? MIXERAT_OSD_EVENT_AIRSPEED_TIMEOUT : MIXERAT_OSD_EVENT_ABORTED); + } + isMixerTransitionMixing_requested = false; mixerProfileAT.phase = MIXERAT_PHASE_IDLE; mixerProfileAT.aborted = wasActive; @@ -834,6 +850,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) mixerProfileAT.hotSwitchDone = true; startServoHandoffFade(); if (!startPostSwitchFade()) { + setMixerATOsdEvent(MIXERAT_OSD_EVENT_DONE); mixerProfileAT.phase = MIXERAT_PHASE_IDLE; mixerProfileAT.request = MIXERAT_REQUEST_NONE; mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; @@ -1136,6 +1153,35 @@ bool NOINLINE mixerATIsActive(void) return mixerProfileAT.phase != MIXERAT_PHASE_IDLE; } +#ifdef USE_AUTO_TRANSITION +bool mixerATGetOsdStatus(mixerProfileATOsdStatus_t *status) +{ + if (!status) { + return false; + } + + const bool active = mixerATIsActive(); + const bool eventActive = mixerATOsdEvent != MIXERAT_OSD_EVENT_NONE && cmp32(millis(), mixerATOsdEventUntil) < 0; + + if (!active && !eventActive) { + return false; + } + + status->active = active; + status->phase = mixerProfileAT.phase; + status->direction = mixerProfileAT.direction; + status->request = mixerProfileAT.request; + status->event = eventActive ? mixerATOsdEvent : MIXERAT_OSD_EVENT_NONE; + + if (!eventActive) { + mixerATOsdEvent = MIXERAT_OSD_EVENT_NONE; + mixerATOsdEventUntil = 0; + } + + return true; +} +#endif + bool mixerATWasAborted(void) { #ifdef USE_AUTO_TRANSITION diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index a3f301e2e00..dc89b128969 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -45,6 +45,7 @@ typedef enum { MIXERAT_REQUEST_MISSION_TO_MC, MIXERAT_REQUEST_MANUAL_TO_FW, MIXERAT_REQUEST_MANUAL_TO_MC, + MIXERAT_REQUEST_FW_TO_MC_PROTECTION, #endif MIXERAT_REQUEST_ABORT, } mixerProfileATRequest_e; @@ -55,6 +56,13 @@ typedef enum { MIXERAT_DIRECTION_TO_FW, MIXERAT_DIRECTION_TO_MC, } mixerProfileATDirection_e; + +typedef enum { + MIXERAT_OSD_EVENT_NONE = 0, + MIXERAT_OSD_EVENT_DONE, + MIXERAT_OSD_EVENT_ABORTED, + MIXERAT_OSD_EVENT_AIRSPEED_TIMEOUT, +} mixerProfileATOsdEvent_e; #endif //mixerProfile Automated Transition PHASE @@ -110,6 +118,17 @@ typedef struct mixerProfileAT_s { timeMs_t transitionTransEndTime; #endif } mixerProfileAT_t; + +#ifdef USE_AUTO_TRANSITION +typedef struct mixerProfileATOsdStatus_s { + bool active; + mixerProfileATState_e phase; + mixerProfileATDirection_e direction; + mixerProfileATRequest_e request; + mixerProfileATOsdEvent_e event; +} mixerProfileATOsdStatus_t; +#endif + extern mixerProfileAT_t mixerProfileAT; bool checkMixerATRequired(mixerProfileATRequest_e required_action); bool mixerATUpdateState(mixerProfileATRequest_e required_action); @@ -123,6 +142,7 @@ float mixerATGetFwAuthorityScale(void); float mixerATGetBlendToFw(void); int16_t mixerATGetTransitionServoInput(void); #ifdef USE_AUTO_TRANSITION +bool mixerATGetOsdStatus(mixerProfileATOsdStatus_t *status); bool mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output); float mixerATGetPostSwitchFadeProgress(void); bool mixerATGetServoHandoffOutput(uint8_t servoIndex, int16_t currentOutput, int16_t *output); diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h index c1df44cb145..d977dd3b3a3 100644 --- a/src/main/flight/mixer_transition_logic.h +++ b/src/main/flight/mixer_transition_logic.h @@ -103,6 +103,9 @@ static inline bool mixerTransitionIsRequestAllowed( case MIXERAT_REQUEST_MISSION_TO_MC: case MIXERAT_REQUEST_MANUAL_TO_MC: return stateAirplane && targetProfileIsMultirotor; + + case MIXERAT_REQUEST_FW_TO_MC_PROTECTION: + return automatedSwitch && stateAirplane && targetProfileIsMultirotor; #endif default: @@ -117,11 +120,30 @@ static inline bool mixerTransitionRequestAllowedDuringFailsafe(mixerProfileATReq case MIXERAT_REQUEST_LAND: return true; +#ifdef USE_AUTO_TRANSITION + case MIXERAT_REQUEST_FW_TO_MC_PROTECTION: + return true; +#endif + default: return false; } } +#ifdef USE_AUTO_TRANSITION +static inline bool mixerTransitionFwToMcProtectionTriggered( + const bool stateAirplane, + const uint16_t thresholdCmS, + const bool trustedAirspeedAvailable, + const float airspeedCmS) +{ + return stateAirplane && + thresholdCmS > 0 && + trustedAirspeedAvailable && + airspeedCmS <= thresholdCmS; +} +#endif + static inline bool mixerTransitionShouldAbortForFailsafe( const mixerProfileATRequest_e requiredAction, const bool postSwitchActive, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8996186a7b1..3a3f53a2300 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -6105,6 +6105,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter const char *invertedInfoMessage = NULL; if (ARMING_FLAG(ARMED)) { + const char *vtolTransitionMessage = osdVtolTransitionMessage(); + if (vtolTransitionMessage) { + ADD_MSG(vtolTransitionMessage); + } + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL NORMALLY, 5 MESSAGES DURING FAILSAFE */ if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 4767ae035d6..692dc2aa35d 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -124,6 +124,22 @@ #define OSD_MSG_ANGLEHOLD_PITCH "(ANGLEHOLD PITCH)" #define OSD_MSG_ANGLEHOLD_LEVEL "(ANGLEHOLD LEVEL)" #define OSD_MSG_MOVE_STICKS "MOVE STICKS TO ABORT" +#define OSD_MSG_VTOL_TRANS_START "VTOL TRANSITION START" +#define OSD_MSG_VTOL_TO_FW "VTOL TO FW" +#define OSD_MSG_VTOL_TO_MC "VTOL TO MC" +#define OSD_MSG_VTOL_MANUAL_TO_FW "VTOL MANUAL TO FW" +#define OSD_MSG_VTOL_MANUAL_TO_MC "VTOL MANUAL TO MC" +#define OSD_MSG_VTOL_MISSION_TO_FW "VTOL MISSION TO FW" +#define OSD_MSG_VTOL_MISSION_TO_MC "VTOL MISSION TO MC" +#define OSD_MSG_VTOL_RTH_TO_FW "VTOL RTH TO FW" +#define OSD_MSG_VTOL_LAND_TO_MC "VTOL LAND TO MC" +#define OSD_MSG_VTOL_SAFE_TO_MC "VTOL LOW SPD TO MC" +#define OSD_MSG_VTOL_FINISH_SWITCH "VTOL FINISHING SWITCH" +#define OSD_MSG_VTOL_RETRY_SCAN "VTOL RETRY SCAN" +#define OSD_MSG_VTOL_RETRY_ALIGN "VTOL RETRY ALIGN" +#define OSD_MSG_VTOL_TRANS_DONE "VTOL TRANSITION DONE" +#define OSD_MSG_VTOL_TRANS_ABORTED "VTOL TRANSITION ABORTED" +#define OSD_MSG_VTOL_AIRSPEED_TO "VTOL AIRSPEED TIMEOUT" #ifdef USE_DEV_TOOLS #define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index 38563ed5d88..539a3502975 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -37,6 +37,9 @@ #include "fc/settings.h" +#include "flight/mixer_profile.h" + +#include "io/osd.h" #include "io/osd_canvas.h" #include "io/osd_common.h" #include "io/osd_grid.h" @@ -70,6 +73,104 @@ int16_t osdGetSpeedFromSelectedSource(void) { return speed; } +#ifdef USE_AUTO_TRANSITION +static const char *osdVtolTransitionRequestMessage(const mixerProfileATRequest_e request, const mixerProfileATDirection_e direction) +{ + switch (request) { + case MIXERAT_REQUEST_RTH: + return OSD_MSG_VTOL_RTH_TO_FW; + + case MIXERAT_REQUEST_LAND: + return OSD_MSG_VTOL_LAND_TO_MC; + + case MIXERAT_REQUEST_FW_TO_MC_PROTECTION: + return OSD_MSG_VTOL_SAFE_TO_MC; + + case MIXERAT_REQUEST_MISSION_TO_FW: + return OSD_MSG_VTOL_MISSION_TO_FW; + + case MIXERAT_REQUEST_MISSION_TO_MC: + return OSD_MSG_VTOL_MISSION_TO_MC; + + case MIXERAT_REQUEST_MANUAL_TO_FW: + return OSD_MSG_VTOL_MANUAL_TO_FW; + + case MIXERAT_REQUEST_MANUAL_TO_MC: + return OSD_MSG_VTOL_MANUAL_TO_MC; + + default: + break; + } + + if (direction == MIXERAT_DIRECTION_TO_FW) { + return OSD_MSG_VTOL_TO_FW; + } + + if (direction == MIXERAT_DIRECTION_TO_MC) { + return OSD_MSG_VTOL_TO_MC; + } + + return NULL; +} +#endif + +const char *osdVtolTransitionMessage(void) +{ +#ifdef USE_AUTO_TRANSITION + const navVtolTransitionOsdState_e navTransitionState = navigationVtolTransitionOsdState(); + switch (navTransitionState) { + case NAV_VTOL_TRANSITION_OSD_RETRY_SCAN: + return OSD_MSG_VTOL_RETRY_SCAN; + + case NAV_VTOL_TRANSITION_OSD_RETRY_ALIGN: + return OSD_MSG_VTOL_RETRY_ALIGN; + + case NAV_VTOL_TRANSITION_OSD_NONE: + default: + break; + } + + mixerProfileATOsdStatus_t status; + if (!mixerATGetOsdStatus(&status)) { + return NULL; + } + + if (!status.active) { + switch (status.event) { + case MIXERAT_OSD_EVENT_DONE: + return OSD_MSG_VTOL_TRANS_DONE; + + case MIXERAT_OSD_EVENT_AIRSPEED_TIMEOUT: + return OSD_MSG_VTOL_AIRSPEED_TO; + + case MIXERAT_OSD_EVENT_ABORTED: + return OSD_MSG_VTOL_TRANS_ABORTED; + + case MIXERAT_OSD_EVENT_NONE: + default: + return NULL; + } + } + + switch (status.phase) { + case MIXERAT_PHASE_TRANSITION_INITIALIZE: + return OSD_MSG_VTOL_TRANS_START; + + case MIXERAT_PHASE_TRANSITIONING: + return osdVtolTransitionRequestMessage(status.request, status.direction); + + case MIXERAT_PHASE_POST_SWITCH_FADE: + return OSD_MSG_VTOL_FINISH_SWITCH; + + case MIXERAT_PHASE_IDLE: + default: + break; + } +#endif + + return NULL; +} + #endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD) #if defined(USE_OSD) diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h index a032747effc..d162b027e58 100644 --- a/src/main/io/osd_common.h +++ b/src/main/io/osd_common.h @@ -46,6 +46,7 @@ typedef struct { PG_DECLARE(osdCommonConfig_t, osdCommonConfig); int16_t osdGetSpeedFromSelectedSource(void); +const char *osdVtolTransitionMessage(void); #endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 086c9a6965f..27e1704e84d 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -995,72 +995,78 @@ static bool djiFormatMessages(char *buff) bool haveMessage = false; char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; if (ARMING_FLAG(ARMED)) { - // Aircraft is armed. We might have up to 6 - // messages to show. - char *messages[6]; + // Aircraft is armed. Keep this bounded because transition, navigation, + // failsafe and normal status messages can be active at the same time. + const char *messages[8]; unsigned messageCount = 0; + #define ADD_DJI_MSG(msg) do { if (messageCount < ARRAYLEN(messages)) messages[messageCount++] = (msg); } while (0) + + const char *vtolTransitionMessage = osdVtolTransitionMessage(); + if (vtolTransitionMessage) { + ADD_DJI_MSG(vtolTransitionMessage); + } if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while being armed too - char *failsafePhaseMessage = osdFailsafePhaseMessage(); - char *failsafeInfoMessage = osdFailsafeInfoMessage(); - char *navStateFSMessage = navigationStateMessage(); + const char *failsafePhaseMessage = osdFailsafePhaseMessage(); + const char *failsafeInfoMessage = osdFailsafeInfoMessage(); + const char *navStateFSMessage = navigationStateMessage(); if (failsafePhaseMessage) { - messages[messageCount++] = failsafePhaseMessage; + ADD_DJI_MSG(failsafePhaseMessage); } if (failsafeInfoMessage) { - messages[messageCount++] = failsafeInfoMessage; + ADD_DJI_MSG(failsafeInfoMessage); } if (navStateFSMessage) { - messages[messageCount++] = navStateFSMessage; + ADD_DJI_MSG(navStateFSMessage); } } else { #ifdef USE_SERIALRX_CRSF if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ && rxLinkStatistics.rfMode == 0) { - messages[messageCount++] = "CRSF LOW RF"; + ADD_DJI_MSG("CRSF LOW RF"); } #endif if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - char *navStateMessage = navigationStateMessage(); + const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { - messages[messageCount++] = navStateMessage; + ADD_DJI_MSG(navStateMessage); } } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { - messages[messageCount++] = "AUTOLAUNCH"; + ADD_DJI_MSG("AUTOLAUNCH"); } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO // when it doesn't require ANGLE mode (required only in FW // right now). If if requires ANGLE, its display is handled // by OSD_FLYMODE. - messages[messageCount++] = "(ALT HOLD)"; + ADD_DJI_MSG("(ALT HOLD)"); } if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { - messages[messageCount++] = "(AUTOTRIM)"; + ADD_DJI_MSG("(AUTOTRIM)"); } if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { - messages[messageCount++] = "(AUTOTUNE)"; + ADD_DJI_MSG("(AUTOTUNE)"); } if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)) { - messages[messageCount++] = "(AUTO LEVEL TRIM)"; + ADD_DJI_MSG("(AUTO LEVEL TRIM)"); } if (FLIGHT_MODE(HEADFREE_MODE)) { - messages[messageCount++] = "(HEADFREE)"; + ADD_DJI_MSG("(HEADFREE)"); } if (FLIGHT_MODE(MANUAL_MODE)) { - messages[messageCount++] = "(MANUAL)"; + ADD_DJI_MSG("(MANUAL)"); } if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { - messages[messageCount++] = "(LAND)"; + ADD_DJI_MSG("(LAND)"); } } } @@ -1070,6 +1076,7 @@ static bool djiFormatMessages(char *buff) strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]); haveMessage = true; } + #undef ADD_DJI_MSG } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { unsigned invalidIndex; // Check if we're unable to arm for some reason diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ea5fca6caa2..af6cc26966c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -335,6 +335,8 @@ typedef enum { } navMissionVtolTransitionDisposition_e; static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; +static mixerProfileATRequest_e navMixerATRequestOverride = MIXERAT_REQUEST_NONE; +static bool navVtolFwToMcProtectionLatched; static navMixerATMissionTransition_t navMixerATMissionTransition; #else static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; @@ -431,6 +433,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState); +#ifdef USE_AUTO_TRANSITION +static bool beginNavigationFwToMcProtectionTransition(void); +#endif #ifdef USE_FW_AUTOLAND static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState); @@ -720,6 +725,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_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_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -763,6 +769,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_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -782,6 +789,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_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -892,6 +900,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_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -939,6 +948,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_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -1397,6 +1407,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState #ifdef USE_AUTO_TRANSITION navMixerATPendingState = NAV_STATE_IDLE; + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + navVtolFwToMcProtectionLatched = false; clearMissionVTOLTransitionState(); #endif resetAltitudeController(false); @@ -1773,6 +1785,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + if (!rthTrackBackSetNewPosition()) { return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; } @@ -1791,7 +1809,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (checkMixerATRequired(MIXERAT_REQUEST_RTH) && (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))){ +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + +#ifdef USE_AUTO_TRANSITION + const bool allowRthMcToFwTransition = !navVtolFwToMcProtectionLatched; +#else + const bool allowRthMcToFwTransition = true; +#endif + + if (allowRthMcToFwTransition && + checkMixerATRequired(MIXERAT_REQUEST_RTH) && + (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))) { return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } @@ -1879,6 +1911,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LAN return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + // Action delay before landing if in FS and option enabled bool pauseLanding = false; navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing; @@ -1924,6 +1962,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(n return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LOITER); setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); @@ -2175,6 +2219,29 @@ static bool hasTrustedPitotAirspeed(float *airspeedCmS) } #endif +static bool beginNavigationFwToMcProtectionTransition(void) +{ + if (navVtolFwToMcProtectionLatched) { + return false; + } + + const uint16_t thresholdCmS = systemConfig()->vtolFwToMcAutoSwitchAirspeed; + float airspeedCmS = 0.0f; + const bool trustedAirspeedAvailable = hasTrustedPitotAirspeed(&airspeedCmS); + + if (!mixerTransitionFwToMcProtectionTriggered(STATE(AIRPLANE), thresholdCmS, trustedAirspeedAvailable, airspeedCmS)) { + return false; + } + + if (!checkMixerATRequired(MIXERAT_REQUEST_FW_TO_MC_PROTECTION)) { + return false; + } + + navMixerATRequestOverride = MIXERAT_REQUEST_FW_TO_MC_PROTECTION; + navVtolFwToMcProtectionLatched = true; + return true; +} + static bool isTransitionRetryToFixedWingRequest(const mixerProfileATRequest_e request) { return request == MIXERAT_REQUEST_MISSION_TO_FW || request == MIXERAT_REQUEST_RTH; @@ -2207,7 +2274,9 @@ static bool canRetryTransitionAfterAirspeedTimeout(const mixerProfileATRequest_e static bool isTransitionToMultirotorRequest(const mixerProfileATRequest_e request) { - return request == MIXERAT_REQUEST_LAND || request == MIXERAT_REQUEST_MISSION_TO_MC; + return request == MIXERAT_REQUEST_LAND || + request == MIXERAT_REQUEST_MISSION_TO_MC || + request == MIXERAT_REQUEST_FW_TO_MC_PROTECTION; } static navigationFSMEvent_t getMcToFwTransitionFailEvent(const mixerProfileATRequest_e request) @@ -2399,6 +2468,12 @@ static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const const bool transitionToFixedWing = ((((uint16_t)waypoint->p3) & configuredUserActionMask) != 0); const mixerProfileATRequest_e requestedAction = transitionToFixedWing ? MIXERAT_REQUEST_MISSION_TO_FW : MIXERAT_REQUEST_MISSION_TO_MC; + // If low-speed protection already selected MC as the safer fallback, do + // not let later mission USER bits immediately send the aircraft back to FW. + if (transitionToFixedWing && navVtolFwToMcProtectionLatched) { + return NAV_MISSION_VTOL_TRANSITION_CONTINUE; + } + // Idempotent mission command: continue immediately if already in requested platform state. if ((transitionToFixedWing && STATE(AIRPLANE)) || (!transitionToFixedWing && STATE(MULTIROTOR))) { return NAV_MISSION_VTOL_TRANSITION_CONTINUE; @@ -2474,6 +2549,26 @@ static void updateMissionTransitionGuidance(void) setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); } + +navVtolTransitionOsdState_e navigationVtolTransitionOsdState(void) +{ + if (posControl.navState != NAV_STATE_MIXERAT_INITIALIZE && + posControl.navState != NAV_STATE_MIXERAT_IN_PROGRESS) { + return NAV_VTOL_TRANSITION_OSD_NONE; + } + + switch (navMixerATMissionTransition.retryStage) { + case NAV_MIXERAT_RETRY_STAGE_SCAN: + return NAV_VTOL_TRANSITION_OSD_RETRY_SCAN; + + case NAV_MIXERAT_RETRY_STAGE_ALIGN: + return NAV_VTOL_TRANSITION_OSD_RETRY_ALIGN; + + case NAV_MIXERAT_RETRY_STAGE_IDLE: + default: + return NAV_VTOL_TRANSITION_OSD_NONE; + } +} #endif static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState) @@ -2562,6 +2657,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na // If no position sensor available - land immediately if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) { +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_HOLD_TIME: case NAV_WP_ACTION_WAYPOINT: @@ -2654,6 +2755,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { // Adjust altitude to waypoint setting setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z); @@ -2846,25 +2953,30 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav #ifdef USE_AUTO_TRANSITION mixerProfileATRequest_e required_action; - switch (navMixerATPendingState) - { - case NAV_STATE_RTH_HEAD_HOME: - required_action = MIXERAT_REQUEST_RTH; - break; - 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; + if (navMixerATRequestOverride != MIXERAT_REQUEST_NONE) { + required_action = navMixerATRequestOverride; + } else { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + required_action = MIXERAT_REQUEST_RTH; + break; + 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; + } } if (!ARMING_FLAG(ARMED) || (FLIGHT_MODE(FAILSAFE_MODE) && mixerTransitionShouldAbortForFailsafe(required_action, false, false))) { mixerATUpdateState(MIXERAT_REQUEST_ABORT); + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; clearMissionVTOLTransitionState(); return NAV_FSM_EVENT_SWITCH_TO_IDLE; } @@ -2878,6 +2990,7 @@ 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); + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; clearMissionVTOLTransitionState(); if (nextEvent != NAV_FSM_EVENT_SWITCH_TO_IDLE) { setupAltitudeController(); @@ -2914,27 +3027,51 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav if (transitionAborted) { nextEvent = getTransitionFailEvent(required_action); } else { - switch (navMixerATPendingState) - { - case NAV_STATE_RTH_HEAD_HOME: - nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; - break; - case NAV_STATE_RTH_LANDING: - nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; - break; - case NAV_STATE_WAYPOINT_PRE_ACTION: - if (missionTransitionWasActive) { + if (required_action == MIXERAT_REQUEST_FW_TO_MC_PROTECTION) { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_TRACKBACK: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH; + break; + case NAV_STATE_RTH_HEAD_HOME: + case NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LOITER_ABOVE_HOME: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH; + break; + case NAV_STATE_WAYPOINT_IN_PROGRESS: + case NAV_STATE_WAYPOINT_HOLD_TIME: nextEvent = NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; + break; + default: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + break; + } + } else { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LANDING: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + break; + case NAV_STATE_WAYPOINT_PRE_ACTION: + if (missionTransitionWasActive) { + nextEvent = NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; + } + break; + default: + break; } - break; - default: - break; } } resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL mixerATUpdateState(MIXERAT_REQUEST_ABORT); + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; clearMissionVTOLTransitionState(); if (nextEvent != NAV_FSM_EVENT_SWITCH_TO_IDLE) { setupAltitudeController(); @@ -2991,6 +3128,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigatio UNUSED(previousState); mixerATUpdateState(MIXERAT_REQUEST_ABORT); #ifdef USE_AUTO_TRANSITION + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; clearMissionVTOLTransitionState(); #endif return NAV_FSM_EVENT_SUCCESS; @@ -5712,6 +5850,8 @@ void navigationInit(void) posControl.navState = NAV_STATE_IDLE; #ifdef USE_AUTO_TRANSITION navMixerATPendingState = NAV_STATE_IDLE; + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + navVtolFwToMcProtectionLatched = false; clearMissionVTOLTransitionState(); #endif diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 36152a7a117..2bc7459dc2a 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -679,6 +679,14 @@ typedef enum { MW_NAV_FLAG_ADJUSTING_ALTITUDE = 1 << 1, } navSystemStatus_Flags_e; +#ifdef USE_AUTO_TRANSITION +typedef enum { + NAV_VTOL_TRANSITION_OSD_NONE = 0, + NAV_VTOL_TRANSITION_OSD_RETRY_SCAN, + NAV_VTOL_TRANSITION_OSD_RETRY_ALIGN, +} navVtolTransitionOsdState_e; +#endif + typedef struct { navSystemStatus_Mode_e mode; navSystemStatus_State_e state; @@ -819,6 +827,9 @@ bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int3 */ bool navigationRTHAllowsLanding(void); bool isWaypointMissionRTHActive(void); +#ifdef USE_AUTO_TRANSITION +navVtolTransitionOsdState_e navigationVtolTransitionOsdState(void); +#endif bool rthClimbStageActiveAndComplete(void); diff --git a/src/test/unit/mixer_transition_policy_unittest.cc b/src/test/unit/mixer_transition_policy_unittest.cc index 5894d25f6f3..0fa1aa2fdea 100644 --- a/src/test/unit/mixer_transition_policy_unittest.cc +++ b/src/test/unit/mixer_transition_policy_unittest.cc @@ -121,6 +121,36 @@ TEST(MixerTransitionPolicyTest, LandRequestNeedsAutomatedSwitchWhenMultirotorPro isMultirotorTypePlatform(profileTypes[targetMcIndex]))); } +TEST(MixerTransitionPolicyTest, NavigationFwToMcProtectionNeedsAutomatedSwitch) +{ + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_FW_TO_MC_PROTECTION, + true, + false, + true, + false, + false, + true)); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_FW_TO_MC_PROTECTION, + true, + false, + true, + true, + false, + true)); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_FW_TO_MC_PROTECTION, + true, + false, + true, + true, + true, + false)); +} + TEST(MixerTransitionPolicyTest, ManualRequestsNeedMixerProfileModeAndMatchingTargetType) { EXPECT_FALSE(mixerTransitionIsRequestAllowed( @@ -160,7 +190,7 @@ TEST(MixerTransitionPolicyTest, ManualRequestsNeedMixerProfileModeAndMatchingTar true)); } -TEST(MixerTransitionPolicyTest, OnlyNavigationOwnedRthAndLandRequestsMayContinueDuringFailsafe) +TEST(MixerTransitionPolicyTest, OnlyNavigationOwnedRequestsMayContinueDuringFailsafe) { EXPECT_TRUE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_RTH)); EXPECT_TRUE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_LAND)); @@ -170,6 +200,7 @@ TEST(MixerTransitionPolicyTest, OnlyNavigationOwnedRthAndLandRequestsMayContinue EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MISSION_TO_MC)); EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MANUAL_TO_FW)); EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MANUAL_TO_MC)); + EXPECT_TRUE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_FW_TO_MC_PROTECTION)); EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_ABORT)); } @@ -177,6 +208,7 @@ TEST(MixerTransitionPolicyTest, FailsafeKeepsPostSwitchOutputRampAfterRequestIsC { EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_RTH, false, false)); EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_LAND, false, false)); + EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_FW_TO_MC_PROTECTION, false, false)); EXPECT_TRUE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, false, false)); EXPECT_TRUE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, true, false)); @@ -185,6 +217,17 @@ TEST(MixerTransitionPolicyTest, FailsafeKeepsPostSwitchOutputRampAfterRequestIsC EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, true, true)); } +TEST(MixerTransitionPolicyTest, FwToMcProtectionAirspeedTriggerNeedsTrustedLowAirspeed) +{ + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(false, 700, true, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, 0, true, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, 700, false, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, 700, true, 701.0f)); + + EXPECT_TRUE(mixerTransitionFwToMcProtectionTriggered(true, 700, true, 700.0f)); + EXPECT_TRUE(mixerTransitionFwToMcProtectionTriggered(true, 700, true, 650.0f)); +} + TEST(MixerTransitionPolicyTest, DynamicScalingDisabledKeepsAllScalesAtFullValues) { const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( From 85aee0d34d20b07079eb907bba85f2882971c3ba Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Fri, 19 Jun 2026 11:55:12 +0300 Subject: [PATCH 38/42] Fix VTOL auto transition handoff edge cases Smooth auto-transition target servo outputs when entering transition so FW/MC surface preview rules do not cause an immediate output jump. Keep a completed manual auto-transition session owning the profile switch until the physical profile switch matches the active target profile. This prevents an AUX gap or middle transition position from switching back to the previous mixer profile after a successful hot-switch. Clear stale transition mixing after a completed auto transition while preserving new transition rising edges, so pusher/lift scaling and transition servo state do not remain active after the state machine is idle. Add unit tests for completed auto-session profile ownership, release behavior, and stale mixing cleanup. --- src/main/flight/mixer_profile.c | 107 ++++++++++++++++-- src/main/flight/mixer_transition_logic.h | 79 ++++++++++++- src/main/navigation/navigation.c | 25 +++- .../navigation_vtol_mission_logic.h | 21 ++++ .../unit/mixer_transition_logic_unittest.cc | 66 +++++++++++ .../unit/mixer_transition_policy_unittest.cc | 86 +++++++++++++- .../navigation_vtol_mission_logic_unittest.cc | 24 ++++ 7 files changed, 388 insertions(+), 20 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 14f74128915..2c263b794df 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -204,6 +204,12 @@ static bool isServoTransitionLinkedInputSource(const uint8_t inputSource) inputSource <= INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS); } +static bool isServoAutoTransitionTargetInputSource(const uint8_t inputSource) +{ + return inputSource >= INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL && + inputSource <= INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS; +} + static uint32_t collectServoTargetMask(const servoMixer_t *rules) { uint32_t mask = 0; @@ -241,6 +247,26 @@ static uint32_t collectTransitionLinkedServoTargetMask(const servoMixer_t *rules return mask; } +static uint32_t collectAutoTransitionTargetServoMask(const servoMixer_t *rules) +{ + uint32_t mask = 0; + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + if (rules[i].rate == 0) { + break; + } + + if (rules[i].targetChannel >= MAX_SUPPORTED_SERVOS || + !isServoAutoTransitionTargetInputSource(rules[i].inputSource)) { + continue; + } + + mask |= 1U << rules[i].targetChannel; + } + + return mask; +} + static bool servoMixerRuleMatches(const servoMixer_t *lhs, const servoMixer_t *rhs) { return lhs->targetChannel == rhs->targetChannel && @@ -332,6 +358,21 @@ static uint32_t collectServoHandoffMask(const int targetProfileIndex, const bool return handoffMask; } +static uint32_t collectTransitionEntryServoHandoffMask(const int targetProfileIndex) +{ + const servoMixer_t *currentRules = mixerServoMixersByIndex(currentMixerProfileIndex); + uint32_t handoffMask = collectAutoTransitionTargetServoMask(currentRules); + + if (targetProfileIndex < 0 || targetProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + return handoffMask; + } + + const servoMixer_t *targetRules = mixerServoMixersByIndex(targetProfileIndex); + handoffMask |= collectAutoTransitionTargetServoMask(targetRules); + + return handoffMask; +} + static void prepareServoHandoffFade(const uint32_t handoffMask) { clearServoHandoffFade(); @@ -387,6 +428,14 @@ static void startServoHandoffFade(void) mixerProfileAT.servoHandoffStartTime = millis(); } +static void startTransitionEntryServoHandoffFade(void) +{ + // Smooth only target-preview surfaces at transition entry. Tilt servos that + // use INPUT_MIXER_TRANSITION already have their own transition trajectory. + prepareServoHandoffFade(collectTransitionEntryServoHandoffMask(nextMixerProfileIndex)); + startServoHandoffFade(); +} + static bool requestTransitionsToFixedWing(const mixerProfileATRequest_e required_action) { return required_action == MIXERAT_REQUEST_RTH || @@ -650,7 +699,7 @@ static bool shouldRequestManualFwToMcProtection(const bool manualControllerEnabl return false; } - return mixerTransitionFwToMcProtectionTriggered(STATE(AIRPLANE), thresholdCmS, true, airspeedCmS); + return mixerTransitionFwToMcProtectionTriggered(ARMING_FLAG(ARMED), STATE(AIRPLANE), thresholdCmS, true, airspeedCmS); } static void updateTransitionScales(void) @@ -827,6 +876,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) mixerProfileAT.request = required_action; mixerProfileAT.direction = directionForRequest(required_action); setMixerProfileAT(); + startTransitionEntryServoHandoffFade(); mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; reprocessState = true; break; @@ -943,12 +993,18 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) const bool transitionModeFallingEdge = !transitionModeActive && manualTransitionModeWasActive; const bool manualTransitionAllowed = (posControl.navState == NAV_STATE_IDLE) || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS); - const bool missionActive = (navGetCurrentStateFlags() & NAV_AUTO_WP) != 0; + const navigationFSMStateFlags_t navStateFlags = navGetCurrentStateFlags(); + const bool missionActive = (navStateFlags & NAV_AUTO_WP) != 0; const bool manualControllerConfigured = currentMixerConfig.manualVtolTransitionController && !missionActive; bool manualControllerEnabled = mixerTransitionManualControllerEnabled(manualControllerConfigured, manualTransitionSessionMode); - const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; const bool mixerProfileModePresent = isModeActivationConditionPresent(BOXMIXERPROFILE); const int requestedProfileIndex = IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1; + const bool keepCompletedAutoSession = mixerTransitionKeepCompletedAutoSession( + manualTransitionSessionMode, + transitionModeFallingEdge, + mixerProfileAT.hotSwitchDone, + currentMixerProfileIndex, + requestedProfileIndex); const bool requestedMultirotorProfile = mixerProfileModePresent && isMultirotorTypePlatform(mixerConfigByIndex(requestedProfileIndex)->platformType); // If low-speed protection already moved the model back to MC, keep direct @@ -956,11 +1012,21 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) const bool fwToMcProtectionOwnsProfileSwitch = manualFwToMcProtectionLatched && STATE(MULTIROTOR) && !requestedMultirotorProfile; + const bool vtolProfilePairConfigured = + (isMultirotorTypePlatform(currentMixerConfig.platformType) && platformTypeConfigured(PLATFORM_AIRPLANE)) || + (currentMixerConfig.platformType == PLATFORM_AIRPLANE && missionTransitionToMultirotorTypeConfigured()); + const bool navigationOwnsProfileSwitch = mixerTransitionNavigationOwnsProfileSwitch( + ARMING_FLAG(ARMED), + vtolProfilePairConfigured, + (navStateFlags & NAV_AUTO_WP) != 0, + (navStateFlags & NAV_AUTO_RTH) != 0, + (navStateFlags & NAV_CTL_LAND) != 0, + (navStateFlags & NAV_MIXERAT) != 0); manualTransitionSessionMode = mixerTransitionUpdateManualSessionMode( manualTransitionSessionMode, transitionModeRisingEdge, - transitionModeFallingEdge, + transitionModeFallingEdge && !keepCompletedAutoSession, manualControllerConfigured, false); @@ -968,6 +1034,13 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) manualFwToMcProtectionLatched = false; } + if (!transitionModeActive && + manualTransitionSessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && + mixerProfileAT.hotSwitchDone && + requestedProfileIndex == currentMixerProfileIndex) { + manualTransitionSessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + } + if (requestedMultirotorProfile || (!mixerAT_inuse && !STATE(MULTIROTOR))) { manualFwToMcProtectionLatched = false; } @@ -990,9 +1063,20 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) mixerAT_inuse = mixerATIsActive(); } + const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; + const bool completedAutoSessionOwnsProfileSwitch = mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + manualTransitionSessionMode, + mixerProfileAT.hotSwitchDone, + currentMixerProfileIndex, + requestedProfileIndex); + if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) { - if (mixerProfileModePresent && !transitionControllerOwnsProfileSwitch && !fwToMcProtectionOwnsProfileSwitch) { + if (mixerProfileModePresent && + !transitionControllerOwnsProfileSwitch && + !completedAutoSessionOwnsProfileSwitch && + !navigationOwnsProfileSwitch && + !fwToMcProtectionOwnsProfileSwitch) { if (requestedProfileIndex != currentMixerProfileIndex) { prepareServoHandoffFade(collectServoHandoffMask(requestedProfileIndex, true)); @@ -1035,6 +1119,12 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) if (!mixerAT_inuse) { isMixerTransitionMixing_requested = false; } + } else if (mixerTransitionShouldClearCompletedAutoMixingRequest( + transitionModeActive, + transitionModeRisingEdge, + mixerAT_inuse, + mixerProfileAT.hotSwitchDone)) { + isMixerTransitionMixing_requested = false; } else if (transitionModeRisingEdge && manualTransitionReadyForEdge && manualTransitionAllowed && !mixerAT_inuse) { manualTransitionReadyForEdge = false; if (STATE(MULTIROTOR)) { @@ -1328,13 +1418,14 @@ bool NOINLINE mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t id return false; } - const float holdScale = 1.0f - constrainf(mixerProfileAT.postSwitchFadeProgress, 0.0f, 1.0f); const int16_t capturedOutput = mixerProfileAT.postSwitchFadeMotorOutput[motorIndex]; const bool fadeToCurrentOutput = (mixerProfileAT.postSwitchFadeToCurrentMotorMask & (1U << motorIndex)) != 0; const int16_t targetOutput = fadeToCurrentOutput ? currentOutput : idleOutput; - const int32_t fadedOutput = lrintf(targetOutput + (capturedOutput - targetOutput) * holdScale); - *output = constrain(fadedOutput, MIN(targetOutput, capturedOutput), MAX(targetOutput, capturedOutput)); + *output = mixerTransitionBlendCapturedMotorOutput( + capturedOutput, + targetOutput, + mixerProfileAT.postSwitchFadeProgress); return true; } diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h index d977dd3b3a3..3c362d5d3ac 100644 --- a/src/main/flight/mixer_transition_logic.h +++ b/src/main/flight/mixer_transition_logic.h @@ -75,6 +75,54 @@ static inline bool mixerTransitionManualControllerEnabled( (sessionMode != MIXER_TRANSITION_MANUAL_SESSION_LEGACY && manualControllerConfigured); } +#ifdef USE_AUTO_TRANSITION +static inline bool mixerTransitionKeepCompletedAutoSession( + mixerTransitionManualSessionMode_e sessionMode, + bool transitionModeFallingEdge, + bool hotSwitchDone, + int currentProfileIndex, + int requestedProfileIndex) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && + transitionModeFallingEdge && + hotSwitchDone && + currentProfileIndex != requestedProfileIndex; +} + +static inline bool mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + mixerTransitionManualSessionMode_e sessionMode, + bool hotSwitchDone, + int currentProfileIndex, + int requestedProfileIndex) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && + hotSwitchDone && + currentProfileIndex != requestedProfileIndex; +} + +static inline bool mixerTransitionShouldClearCompletedAutoMixingRequest( + bool transitionModeActive, + bool transitionModeRisingEdge, + bool autoTransitionActive, + bool hotSwitchDone) +{ + return transitionModeActive && !transitionModeRisingEdge && !autoTransitionActive && hotSwitchDone; +} + +static inline bool mixerTransitionNavigationOwnsProfileSwitch( + bool armed, + bool vtolProfilePairConfigured, + bool navWaypointActive, + bool navRthActive, + bool navLandingActive, + bool navMixerAtActive) +{ + return armed && + vtolProfilePairConfigured && + (navWaypointActive || navRthActive || navLandingActive || navMixerAtActive); +} +#endif + static inline bool mixerTransitionIsRequestAllowed( mixerProfileATRequest_e requiredAction, bool stateAirplane, @@ -132,12 +180,14 @@ static inline bool mixerTransitionRequestAllowedDuringFailsafe(mixerProfileATReq #ifdef USE_AUTO_TRANSITION static inline bool mixerTransitionFwToMcProtectionTriggered( + const bool armed, const bool stateAirplane, const uint16_t thresholdCmS, const bool trustedAirspeedAvailable, const float airspeedCmS) { - return stateAirplane && + return armed && + stateAirplane && thresholdCmS > 0 && trustedAirspeedAvailable && airspeedCmS <= thresholdCmS; @@ -191,6 +241,11 @@ static inline bool mixerTransitionServoHandoffHoldActive(uint16_t holdDurationMs return holdDurationMs > 0 && elapsedMs < holdDurationMs; } +static inline int16_t mixerTransitionRoundFloatToInt16(float value) +{ + return (int16_t)(value >= 0.0f ? value + 0.5f : value - 0.5f); +} + static inline float mixerTransitionResolveHandoffProgress( bool dynamicMixerEnabled, bool usedAirspeed, @@ -215,6 +270,28 @@ static inline float mixerTransitionBlendScale(float from, float to, float progre return from + (to - from) * mixerTransitionClamp(progress, 0.0f, 1.0f); } +static inline int16_t mixerTransitionBlendCapturedMotorOutput( + int16_t capturedOutput, + int16_t targetOutput, + float progress) +{ + const float holdScale = 1.0f - mixerTransitionClamp(progress, 0.0f, 1.0f); + const int32_t blendedOutput = mixerTransitionRoundFloatToInt16( + targetOutput + (capturedOutput - targetOutput) * holdScale); + const int16_t low = targetOutput < capturedOutput ? targetOutput : capturedOutput; + const int16_t high = targetOutput > capturedOutput ? targetOutput : capturedOutput; + + if (blendedOutput < low) { + return low; + } + + if (blendedOutput > high) { + return high; + } + + return (int16_t)blendedOutput; +} + static inline mixerTransitionScaleState_t mixerTransitionComputeScales( bool dynamicMixerEnabled, mixerProfileATDirection_e direction, diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index af6cc26966c..2bf82a9b034 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -331,6 +331,7 @@ typedef enum { NAV_MISSION_VTOL_TRANSITION_CONTINUE, NAV_MISSION_VTOL_TRANSITION_WAIT, NAV_MISSION_VTOL_TRANSITION_START, + NAV_MISSION_VTOL_TRANSITION_FAIL_ACTION, NAV_MISSION_VTOL_TRANSITION_REJECT, } navMissionVtolTransitionDisposition_e; @@ -875,7 +876,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_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_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX @@ -2229,7 +2232,7 @@ static bool beginNavigationFwToMcProtectionTransition(void) float airspeedCmS = 0.0f; const bool trustedAirspeedAvailable = hasTrustedPitotAirspeed(&airspeedCmS); - if (!mixerTransitionFwToMcProtectionTriggered(STATE(AIRPLANE), thresholdCmS, trustedAirspeedAvailable, airspeedCmS)) { + if (!mixerTransitionFwToMcProtectionTriggered(ARMING_FLAG(ARMED), STATE(AIRPLANE), thresholdCmS, trustedAirspeedAvailable, airspeedCmS)) { return false; } @@ -2504,13 +2507,20 @@ static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const } const flyingPlatformType_e targetPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; - if ((transitionToFixedWing && targetPlatformType != PLATFORM_AIRPLANE) || - (!transitionToFixedWing && !isMissionTransitionToMultirotorType(targetPlatformType))) { + const bool targetPlatformMatchesRequest = transitionToFixedWing ? + targetPlatformType == PLATFORM_AIRPLANE : + isMissionTransitionToMultirotorType(targetPlatformType); + const navMissionVtolTransitionStartValidation_e startValidation = navMissionVtolTransitionStartValidation( + targetPlatformMatchesRequest, + checkMixerATRequired(requestedAction)); + + if (startValidation == NAV_MISSION_VTOL_START_VALIDATION_REJECT) { return NAV_MISSION_VTOL_TRANSITION_REJECT; } - if (!checkMixerATRequired(requestedAction)) { - return NAV_MISSION_VTOL_TRANSITION_REJECT; + if (startValidation == NAV_MISSION_VTOL_START_VALIDATION_FAIL_ACTION) { + navMixerATMissionTransition.request = requestedAction; + return NAV_MISSION_VTOL_TRANSITION_FAIL_ACTION; } int32_t transitionHeading = posControl.actualState.yaw; @@ -2600,6 +2610,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav if (transitionAction == NAV_MISSION_VTOL_TRANSITION_START) { return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_FAIL_ACTION) { + const navigationFSMEvent_t failEvent = getTransitionFailEvent(navMixerATMissionTransition.request); + clearMissionVTOLTransitionState(); + return failEvent; + } if (transitionAction == NAV_MISSION_VTOL_TRANSITION_REJECT) { return NAV_FSM_EVENT_ERROR; } diff --git a/src/main/navigation/navigation_vtol_mission_logic.h b/src/main/navigation/navigation_vtol_mission_logic.h index ef51285d4d7..f90b02afb35 100644 --- a/src/main/navigation/navigation_vtol_mission_logic.h +++ b/src/main/navigation/navigation_vtol_mission_logic.h @@ -25,6 +25,12 @@ typedef enum { NAV_MISSION_VTOL_PRECONDITION_REJECT, } navMissionVtolTransitionPrecondition_e; +typedef enum { + NAV_MISSION_VTOL_START_VALIDATION_READY = 0, + NAV_MISSION_VTOL_START_VALIDATION_FAIL_ACTION, + NAV_MISSION_VTOL_START_VALIDATION_REJECT, +} navMissionVtolTransitionStartValidation_e; + static inline navMissionVtolTransitionPrecondition_e navMissionVtolTransitionPreconditionDisposition( const bool armed, const bool failsafeActive, @@ -49,3 +55,18 @@ static inline navMissionVtolTransitionPrecondition_e navMissionVtolTransitionPre return NAV_MISSION_VTOL_PRECONDITION_READY; } + +static inline navMissionVtolTransitionStartValidation_e navMissionVtolTransitionStartValidation( + const bool targetPlatformMatchesRequest, + const bool transitionRequestAllowed) +{ + if (!targetPlatformMatchesRequest) { + return NAV_MISSION_VTOL_START_VALIDATION_REJECT; + } + + if (!transitionRequestAllowed) { + return NAV_MISSION_VTOL_START_VALIDATION_FAIL_ACTION; + } + + return NAV_MISSION_VTOL_START_VALIDATION_READY; +} diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc index 6b0d217a00c..6d19c0de6eb 100644 --- a/src/test/unit/mixer_transition_logic_unittest.cc +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -43,6 +43,72 @@ TEST(MixerTransitionLogicTest, AutoManualSessionStaysAutoAcrossProfileChanges) EXPECT_TRUE(mixerTransitionManualControllerEnabled(false, sessionMode)); } +TEST(MixerTransitionLogicTest, CompletedAutoSessionSurvivesProfileModeGapAfterHotSwitch) +{ + EXPECT_TRUE(mixerTransitionKeepCompletedAutoSession( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + true, + 0, + 1)); + + EXPECT_TRUE(mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + 0, + 1)); +} + +TEST(MixerTransitionLogicTest, CompletedAutoSessionReleasesWhenSwitchMatchesActiveProfile) +{ + EXPECT_FALSE(mixerTransitionKeepCompletedAutoSession( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + true, + 0, + 0)); + + EXPECT_FALSE(mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + 0, + 0)); + + EXPECT_FALSE(mixerTransitionKeepCompletedAutoSession( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + false, + 0, + 1)); +} + +TEST(MixerTransitionLogicTest, CompletedAutoSessionClearsStaleMixingWhileSwitchRemainsInTransition) +{ + EXPECT_TRUE(mixerTransitionShouldClearCompletedAutoMixingRequest( + true, + false, + false, + true)); + + EXPECT_FALSE(mixerTransitionShouldClearCompletedAutoMixingRequest( + true, + false, + true, + true)); + + EXPECT_FALSE(mixerTransitionShouldClearCompletedAutoMixingRequest( + false, + false, + false, + true)); + + EXPECT_FALSE(mixerTransitionShouldClearCompletedAutoMixingRequest( + true, + true, + false, + true)); +} + TEST(MixerTransitionLogicTest, LegacySessionIgnoresAutoControllerAfterProfileHotSwitch) { mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; diff --git a/src/test/unit/mixer_transition_policy_unittest.cc b/src/test/unit/mixer_transition_policy_unittest.cc index 0fa1aa2fdea..e5a51285909 100644 --- a/src/test/unit/mixer_transition_policy_unittest.cc +++ b/src/test/unit/mixer_transition_policy_unittest.cc @@ -219,13 +219,73 @@ TEST(MixerTransitionPolicyTest, FailsafeKeepsPostSwitchOutputRampAfterRequestIsC TEST(MixerTransitionPolicyTest, FwToMcProtectionAirspeedTriggerNeedsTrustedLowAirspeed) { - EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(false, 700, true, 650.0f)); - EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, 0, true, 650.0f)); - EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, 700, false, 650.0f)); - EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, 700, true, 701.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(false, true, 700, true, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, false, 700, true, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, true, 0, true, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, true, 700, false, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, true, 700, true, 701.0f)); + + EXPECT_TRUE(mixerTransitionFwToMcProtectionTriggered(true, true, 700, true, 700.0f)); + EXPECT_TRUE(mixerTransitionFwToMcProtectionTriggered(true, true, 700, true, 650.0f)); +} + +TEST(MixerTransitionPolicyTest, NavigationOwnsProfileSwitchOnlyForArmedVtolAutoStates) +{ + EXPECT_FALSE(mixerTransitionNavigationOwnsProfileSwitch( + false, + true, + true, + false, + false, + false)); + + EXPECT_FALSE(mixerTransitionNavigationOwnsProfileSwitch( + true, + false, + true, + false, + false, + false)); + + EXPECT_FALSE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + false, + false, + false, + false)); - EXPECT_TRUE(mixerTransitionFwToMcProtectionTriggered(true, 700, true, 700.0f)); - EXPECT_TRUE(mixerTransitionFwToMcProtectionTriggered(true, 700, true, 650.0f)); + EXPECT_TRUE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + true, + false, + false, + false)); + + EXPECT_TRUE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + false, + true, + false, + false)); + + EXPECT_TRUE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + false, + false, + true, + false)); + + EXPECT_TRUE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + false, + false, + false, + true)); } TEST(MixerTransitionPolicyTest, DynamicScalingDisabledKeepsAllScalesAtFullValues) @@ -300,6 +360,20 @@ TEST(MixerTransitionPolicyTest, MotorRampProgressFallsBackToFullWhenDisabledOrZe EXPECT_FLOAT_EQ(0.40f, mixerTransitionComputeMotorRampProgress(true, 500, 200)); } +TEST(MixerTransitionPolicyTest, PostSwitchMotorOutputBlendIsBounded) +{ + EXPECT_EQ(1800, mixerTransitionBlendCapturedMotorOutput(1800, 1000, 0.0f)); + EXPECT_EQ(1400, mixerTransitionBlendCapturedMotorOutput(1800, 1000, 0.5f)); + EXPECT_EQ(1000, mixerTransitionBlendCapturedMotorOutput(1800, 1000, 1.0f)); + + EXPECT_EQ(1000, mixerTransitionBlendCapturedMotorOutput(1000, 1800, 0.0f)); + EXPECT_EQ(1400, mixerTransitionBlendCapturedMotorOutput(1000, 1800, 0.5f)); + EXPECT_EQ(1800, mixerTransitionBlendCapturedMotorOutput(1000, 1800, 1.0f)); + + EXPECT_EQ(1800, mixerTransitionBlendCapturedMotorOutput(1800, 1000, -0.25f)); + EXPECT_EQ(1000, mixerTransitionBlendCapturedMotorOutput(1800, 1000, 1.25f)); +} + TEST(MixerTransitionPolicyTest, DirectionNoneKeepsScalesAndHotSwitchIdle) { const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( diff --git a/src/test/unit/navigation_vtol_mission_logic_unittest.cc b/src/test/unit/navigation_vtol_mission_logic_unittest.cc index b70966ea6e6..e1cb1c79f9a 100644 --- a/src/test/unit/navigation_vtol_mission_logic_unittest.cc +++ b/src/test/unit/navigation_vtol_mission_logic_unittest.cc @@ -111,3 +111,27 @@ TEST(NavigationVtolMissionLogicTest, HardSafetyAndConfigurationProblemsReject) false, false)); } + +TEST(NavigationVtolMissionLogicTest, StartValidationRejectsBadTargetProfile) +{ + EXPECT_EQ(NAV_MISSION_VTOL_START_VALIDATION_REJECT, + navMissionVtolTransitionStartValidation( + false, + true)); +} + +TEST(NavigationVtolMissionLogicTest, StartValidationUsesFailActionWhenRequestCannotStart) +{ + EXPECT_EQ(NAV_MISSION_VTOL_START_VALIDATION_FAIL_ACTION, + navMissionVtolTransitionStartValidation( + true, + false)); +} + +TEST(NavigationVtolMissionLogicTest, StartValidationAllowsValidTransitionStart) +{ + EXPECT_EQ(NAV_MISSION_VTOL_START_VALIDATION_READY, + navMissionVtolTransitionStartValidation( + true, + true)); +} From 64dbf9cc54dde5f8587c3c14f998f085045140b4 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Fri, 19 Jun 2026 16:50:01 +0300 Subject: [PATCH 39/42] Protect VTOL MC stabilization from MC braking overlap Prevent MR braking mode from taking over XY target handling while VTOL MC protection is active, so VTOL capture can keep controlling the hold target without being blocked by NAV_CRUISE_BRAKING. Add small helpers for current position hold target and braking state visibility, and cover the suppression policy with a unit test. --- src/main/navigation/navigation_multicopter.c | 42 ++++++++++++++++--- src/main/navigation/navigation_private.h | 2 + .../navigation_vtol_mc_protection_logic.h | 5 +++ .../unit/vtol_mc_protection_logic_unittest.cc | 6 +++ 4 files changed, 50 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 1512d641dcb..576467fd116 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -78,6 +78,11 @@ typedef struct mcLandingProbeState_s { static mcLandingProbeState_t mcLandingProbe; +static void setMulticopterCurrentPositionAsHoldTarget(navSetWaypointFlags_t useMask) +{ + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, useMask); +} + static void resetMulticopterLandingProbe(void) { mcLandingProbe = (mcLandingProbeState_t){ 0 }; @@ -392,17 +397,41 @@ void resetMulticopterBrakingMode(void) DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED); } +bool navigationMulticopterBrakingActive(void) +{ +#ifdef USE_MR_BRAKING_MODE + return STATE(NAV_CRUISE_BRAKING); +#else + return false; +#endif +} + +bool navigationMulticopterBrakingBoostActive(void) +{ +#ifdef USE_MR_BRAKING_MODE + return STATE(NAV_CRUISE_BRAKING_BOOST); +#else + return false; +#endif +} + static void processMulticopterBrakingMode(const bool isAdjusting) { #ifdef USE_MR_BRAKING_MODE static uint32_t brakingModeDisengageAt = 0; static uint32_t brakingBoostModeDisengageAt = 0; + const bool brakingSuppressed = vtolMcProtectionSuppressesMulticopterBrakingMode(navigationVtolMcProtectionIsNavActive()); if (!(NAV_Status.state == MW_NAV_STATE_NONE || NAV_Status.state == MW_NAV_STATE_HOLD_INFINIT)) { resetMulticopterBrakingMode(); return; } + if (brakingSuppressed) { + resetMulticopterBrakingMode(); + return; + } + const bool brakingEntryAllowed = IS_RC_MODE_ACTIVE(BOXBRAKING) && !STATE(NAV_CRUISE_BRAKING_LOCKED) && @@ -422,7 +451,7 @@ static void processMulticopterBrakingMode(const bool isAdjusting) * Set currnt position and target position * Enabling NAV_CRUISE_BRAKING locks other routines from setting position! */ - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); + setMulticopterCurrentPositionAsHoldTarget(NAV_POS_UPDATE_XY); ENABLE_STATE(NAV_CRUISE_BRAKING_LOCKED); ENABLE_STATE(NAV_CRUISE_BRAKING); @@ -470,7 +499,7 @@ static void processMulticopterBrakingMode(const bool isAdjusting) * When braking is done, store current position as desired one * We do not want to go back to the place where braking has started */ - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); + setMulticopterCurrentPositionAsHoldTarget(NAV_POS_UPDATE_XY); } #else UNUSED(isAdjusting); @@ -680,7 +709,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA float maxAccelChange = US2S(deltaMicros) * MC_POS_CONTROL_JERK_LIMIT_CMSSS; //When braking, raise jerk limit even if we are not boosting acceleration #ifdef USE_MR_BRAKING_MODE - if (STATE(NAV_CRUISE_BRAKING)) { + if (navigationMulticopterBrakingActive() && + !vtolMcProtectionSuppressesMulticopterBrakingMode(navigationVtolMcProtectionIsNavActive())) { maxAccelChange = maxAccelChange * 2; } #endif @@ -746,7 +776,9 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA #ifdef USE_MR_BRAKING_MODE //Boost required accelerations - if (STATE(NAV_CRUISE_BRAKING_BOOST) && multicopterPosXyCoefficients.breakingBoostFactor > 0.0f) { + if (navigationMulticopterBrakingBoostActive() && + !vtolMcProtectionSuppressesMulticopterBrakingMode(navigationVtolMcProtectionIsNavActive()) && + multicopterPosXyCoefficients.breakingBoostFactor > 0.0f) { //Scale boost factor according to speed const float boostFactor = constrainf( @@ -817,7 +849,7 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) // If we have new position data - update velocity and acceleration controllers if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { if (navigationVtolMcProtectionApplyCapture(navGetCurrentStateFlags())) { - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); + setMulticopterCurrentPositionAsHoldTarget(NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); } // Get max speed for current NAV mode diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index d2077bc99c1..c5440c7a7cf 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -583,6 +583,8 @@ void resetMulticopterAltitudeController(void); void resetMulticopterPositionController(void); void resetMulticopterHeadingController(void); void resetMulticopterBrakingMode(void); +bool navigationMulticopterBrakingActive(void); +bool navigationMulticopterBrakingBoostActive(void); bool adjustMulticopterAltitudeFromRCInput(void); bool adjustMulticopterHeadingFromRCInput(void); diff --git a/src/main/navigation/navigation_vtol_mc_protection_logic.h b/src/main/navigation/navigation_vtol_mc_protection_logic.h index d3ebd831ac8..fed788d50eb 100644 --- a/src/main/navigation/navigation_vtol_mc_protection_logic.h +++ b/src/main/navigation/navigation_vtol_mc_protection_logic.h @@ -86,6 +86,11 @@ static inline uint16_t vtolMcProtectionSettleAttitudeLimitDeciDeg(const uint8_t return (uint16_t)limitedAngleDeg * 10U; } +static inline bool vtolMcProtectionSuppressesMulticopterBrakingMode(const bool active) +{ + return active; +} + static inline uint16_t vtolMcProtectionBailoutAngleLimitDeciDeg(const uint8_t navMcBankAngleDeg) { uint8_t bailoutAngleDeg = navMcBankAngleDeg + VTOL_MC_BAILOUT_ANGLE_EXTRA_DEG; diff --git a/src/test/unit/vtol_mc_protection_logic_unittest.cc b/src/test/unit/vtol_mc_protection_logic_unittest.cc index 50bb6445b82..5eb8c424713 100644 --- a/src/test/unit/vtol_mc_protection_logic_unittest.cc +++ b/src/test/unit/vtol_mc_protection_logic_unittest.cc @@ -374,6 +374,12 @@ TEST(VtolMcProtectionLogicTest, BailoutAngleLimitUsesBankAngleWithSafeClamps) EXPECT_EQ(600, vtolMcProtectionBailoutAngleLimitDeciDeg(80)); } +TEST(VtolMcProtectionLogicTest, ActiveProtectionSuppressesMulticopterBrakingMode) +{ + EXPECT_FALSE(vtolMcProtectionSuppressesMulticopterBrakingMode(false)); + EXPECT_TRUE(vtolMcProtectionSuppressesMulticopterBrakingMode(true)); +} + TEST(VtolMcProtectionLogicTest, CommandShapingIsContinuousAndPreservesSign) { EXPECT_FLOAT_EQ(1.0f, vtolMcProtectionCommandScaleForSpeed(250.0f)); From 26d3df4cc2a5e13cd47b9da92b1619dab386db0a Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Mon, 22 Jun 2026 11:22:56 +0300 Subject: [PATCH 40/42] Fix manual VTOL endpoint switch after auto transition Release completed manual auto-transition sessions when MIXER TRANSITION is switched off, so moving the switch to an endpoint after a completed transition allows the normal direct profile switch path instead of keeping the old completed session latched. Update the transition logic unit test to cover the falling-edge endpoint release behavior. --- src/main/flight/mixer_transition_logic.h | 2 +- src/test/unit/mixer_transition_logic_unittest.cc | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h index 3c362d5d3ac..e361b547023 100644 --- a/src/main/flight/mixer_transition_logic.h +++ b/src/main/flight/mixer_transition_logic.h @@ -84,7 +84,7 @@ static inline bool mixerTransitionKeepCompletedAutoSession( int requestedProfileIndex) { return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && - transitionModeFallingEdge && + !transitionModeFallingEdge && hotSwitchDone && currentProfileIndex != requestedProfileIndex; } diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc index 6d19c0de6eb..7e71edbe3a2 100644 --- a/src/test/unit/mixer_transition_logic_unittest.cc +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -43,20 +43,21 @@ TEST(MixerTransitionLogicTest, AutoManualSessionStaysAutoAcrossProfileChanges) EXPECT_TRUE(mixerTransitionManualControllerEnabled(false, sessionMode)); } -TEST(MixerTransitionLogicTest, CompletedAutoSessionSurvivesProfileModeGapAfterHotSwitch) +TEST(MixerTransitionLogicTest, CompletedAutoSessionReleasesOnFallingEdgeToEndpoint) { - EXPECT_TRUE(mixerTransitionKeepCompletedAutoSession( + EXPECT_FALSE(mixerTransitionKeepCompletedAutoSession( MIXER_TRANSITION_MANUAL_SESSION_AUTO, true, true, 0, 1)); - EXPECT_TRUE(mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, mixerTransitionUpdateManualSessionMode( MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, true, - 0, - 1)); + true, + false)); } TEST(MixerTransitionLogicTest, CompletedAutoSessionReleasesWhenSwitchMatchesActiveProfile) From 83a4b6265815ae8f1133a014b15ea66d9f20d865 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Wed, 24 Jun 2026 11:59:23 +0300 Subject: [PATCH 41/42] Fix VTOL manual switch handoff and OSD reminders Prevent direct manual profile switches and direct transition aborts from using auto-transition servo handoff smoothing. Direct switch now clears transition handoff state and switches immediately to the requested profile. Add blinking VTOL OSD reminders after automatic manual transitions and after NAV/mission handback when the physical mixer switch does not match the active profile. Show MOVE SWITCH TO FW/MC until the pilot confirms the active endpoint, including DJI HD OSD support. Hold the active mixer profile after NAV releases profile ownership if the manual switch is still in transition position or points to a different profile, avoiding an unexpected direct switch when a mission ends or WP mode is disabled. Add unit tests for manual switch reminders, navigation handback hold/clear logic, and remove the obsolete direct-switch servo hold test. --- src/main/flight/mixer_profile.c | 129 +++++++------- src/main/flight/mixer_profile.h | 1 + src/main/flight/mixer_transition_logic.h | 55 +++++- src/main/io/osd.c | 5 +- src/main/io/osd.h | 2 + src/main/io/osd_common.c | 23 +++ src/main/io/osd_common.h | 1 + src/main/io/osd_dji_hd.c | 11 +- .../unit/mixer_transition_logic_unittest.cc | 162 +++++++++++++++++- 9 files changed, 314 insertions(+), 75 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 2c263b794df..63735c4b799 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -38,7 +38,6 @@ #include "build/debug.h" #ifdef USE_AUTO_TRANSITION -#define MIXER_TRANSITION_DIRECT_SWITCH_SERVO_HOLD_MS 300 #define MIXER_TRANSITION_OSD_EVENT_DISPLAY_MS 3000 #endif @@ -56,6 +55,9 @@ static bool manualFwToMcProtectionLatched; static int16_t mixerTransitionServoInput; static mixerProfileATOsdEvent_e mixerATOsdEvent; static timeMs_t mixerATOsdEventUntil; +static mixerProfileATDirection_e mixerATOsdSwitchReminderDirection; +static bool navigationProfileSwitchWasOwned; +static bool navigationProfileHandbackPending; #endif // Keep PG version split because USE_AUTO_TRANSITION changes the stored mixer profile layout only on >512 KB targets. @@ -187,6 +189,26 @@ static void setMixerATOsdEvent(const mixerProfileATOsdEvent_e event) mixerATOsdEventUntil = event == MIXERAT_OSD_EVENT_NONE ? 0 : millis() + MIXER_TRANSITION_OSD_EVENT_DISPLAY_MS; } +static void updateMixerATSwitchReminder( + const bool transitionModeActive, + const int requestedProfileIndex) +{ + mixerATOsdSwitchReminderDirection = mixerTransitionManualSwitchReminderDirection( + manualTransitionSessionMode, + mixerATIsActive(), + mixerProfileAT.hotSwitchDone, + transitionModeActive, + currentMixerProfileIndex, + requestedProfileIndex, + isMultirotorTypePlatform(currentMixerConfig.platformType)); + + if (mixerATOsdSwitchReminderDirection == MIXERAT_DIRECTION_NONE && navigationProfileHandbackPending) { + mixerATOsdSwitchReminderDirection = isMultirotorTypePlatform(currentMixerConfig.platformType) ? + MIXERAT_DIRECTION_TO_MC : + MIXERAT_DIRECTION_TO_FW; + } +} + static void clearServoHandoffFade(void) { mixerProfileAT.servoHandoffMask = 0; @@ -395,28 +417,6 @@ static void prepareServoHandoffFade(const uint32_t handoffMask) } } -static void prepareServoHandoffHold(const uint32_t handoffMask, const uint16_t holdDurationMs) -{ - clearServoHandoffFade(); - - if (handoffMask == 0 || holdDurationMs == 0) { - return; - } - - mixerProfileAT.servoHandoffMask = handoffMask; - mixerProfileAT.servoHandoffDurationMs = getServoHandoffDurationMs(); - mixerProfileAT.servoHandoffHoldDurationMs = holdDurationMs; - mixerProfileAT.servoHandoffHoldStartTime = millis(); - - for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - if ((handoffMask & (1U << i)) == 0) { - continue; - } - - mixerProfileAT.servoHandoffOutput[i] = servo[i]; - } -} - static void startServoHandoffFade(void) { if (mixerProfileAT.servoHandoffMask == 0 || mixerProfileAT.servoHandoffDurationMs == 0) { @@ -436,6 +436,14 @@ static void startTransitionEntryServoHandoffFade(void) startServoHandoffFade(); } +static bool outputProfileDirectSwitch(const int profileIndex) +{ + clearServoHandoffFade(); + const bool switched = outputProfileHotSwitch(profileIndex); + clearServoHandoffFade(); + return switched; +} + static bool requestTransitionsToFixedWing(const mixerProfileATRequest_e required_action) { return required_action == MIXERAT_REQUEST_RTH || @@ -720,15 +728,15 @@ static void updateTransitionScales(void) mixerProfileAT.fwAuthorityScale = scales.fwAuthorityScale; } -static void abortTransition(const bool byAirspeedTimeout, const bool holdServoOutputsForDirectSwitch) +static void abortTransition(const bool byAirspeedTimeout, const bool directSwitchAbort) { const bool wasActive = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; const bool hotSwitchAlreadyDone = mixerProfileAT.hotSwitchDone; - const uint32_t servoHandoffMask = (wasActive && !hotSwitchAlreadyDone) ? + const uint32_t servoHandoffMask = (wasActive && !hotSwitchAlreadyDone && !directSwitchAbort) ? collectServoHandoffMask(nextMixerProfileIndex, false) : 0; - if (servoHandoffMask != 0 && !holdServoOutputsForDirectSwitch) { + if (servoHandoffMask != 0) { prepareServoHandoffFade(servoHandoffMask); } @@ -748,13 +756,7 @@ static void abortTransition(const bool byAirspeedTimeout, const bool holdServoOu mixerProfileAT.transitionStartAirspeedCmS = 0.0f; resetTransitionScales(); - if (servoHandoffMask != 0 && holdServoOutputsForDirectSwitch) { - // A manual direct switch may report MIXERTRANSITION OFF a few control - // cycles before MIXERPROFILE changes. Hold the real servo output so - // transition-linked tilt servos do not briefly move back toward the - // source profile before the destination profile takes ownership. - prepareServoHandoffHold(servoHandoffMask, MIXER_TRANSITION_DIRECT_SWITCH_SERVO_HOLD_MS); - } else if (servoHandoffMask != 0) { + if (servoHandoffMask != 0) { startServoHandoffFade(); } } @@ -1063,6 +1065,28 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) mixerAT_inuse = mixerATIsActive(); } + if (!ARMING_FLAG(ARMED)) { + navigationProfileSwitchWasOwned = false; + navigationProfileHandbackPending = false; + } else if (mixerTransitionNavigationHandbackShouldClear( + navigationOwnsProfileSwitch, + transitionModeRisingEdge, + transitionModeActive, + currentMixerProfileIndex, + requestedProfileIndex)) { + navigationProfileHandbackPending = false; + } else if (mixerTransitionNavigationHandbackShouldHoldProfile( + navigationProfileSwitchWasOwned, + navigationOwnsProfileSwitch, + mixerProfileModePresent, + mixerAT_inuse, + transitionModeActive, + currentMixerProfileIndex, + requestedProfileIndex)) { + navigationProfileHandbackPending = true; + } + navigationProfileSwitchWasOwned = navigationOwnsProfileSwitch; + const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; const bool completedAutoSessionOwnsProfileSwitch = mixerTransitionCompletedAutoSessionOwnsProfileSwitch( manualTransitionSessionMode, @@ -1076,15 +1100,10 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) !transitionControllerOwnsProfileSwitch && !completedAutoSessionOwnsProfileSwitch && !navigationOwnsProfileSwitch && + !navigationProfileHandbackPending && !fwToMcProtectionOwnsProfileSwitch) { if (requestedProfileIndex != currentMixerProfileIndex) { - prepareServoHandoffFade(collectServoHandoffMask(requestedProfileIndex, true)); - - if (outputProfileHotSwitch(requestedProfileIndex)) { - startServoHandoffFade(); - } else { - clearServoHandoffFade(); - } + outputProfileDirectSwitch(requestedProfileIndex); } } } @@ -1141,6 +1160,12 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { abortTransition(false, true); mixerAT_inuse = false; + if (!FLIGHT_MODE(FAILSAFE_MODE) && + mixerProfileModePresent && + !navigationOwnsProfileSwitch && + requestedProfileIndex != currentMixerProfileIndex) { + outputProfileDirectSwitch(requestedProfileIndex); + } } if (mixerAT_inuse && @@ -1150,6 +1175,8 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) } } + updateMixerATSwitchReminder(transitionModeActive, requestedProfileIndex); + manualTransitionModeWasActive = transitionModeActive; isMixerTransitionMixing = isMixerTransitionMixing_requested && @@ -1252,8 +1279,9 @@ bool mixerATGetOsdStatus(mixerProfileATOsdStatus_t *status) const bool active = mixerATIsActive(); const bool eventActive = mixerATOsdEvent != MIXERAT_OSD_EVENT_NONE && cmp32(millis(), mixerATOsdEventUntil) < 0; + const bool switchReminderActive = mixerATOsdSwitchReminderDirection != MIXERAT_DIRECTION_NONE; - if (!active && !eventActive) { + if (!active && !eventActive && !switchReminderActive) { return false; } @@ -1262,6 +1290,7 @@ bool mixerATGetOsdStatus(mixerProfileATOsdStatus_t *status) status->direction = mixerProfileAT.direction; status->request = mixerProfileAT.request; status->event = eventActive ? mixerATOsdEvent : MIXERAT_OSD_EVENT_NONE; + status->switchReminderDirection = switchReminderActive ? mixerATOsdSwitchReminderDirection : MIXERAT_DIRECTION_NONE; if (!eventActive) { mixerATOsdEvent = MIXERAT_OSD_EVENT_NONE; @@ -1376,24 +1405,6 @@ bool mixerATGetServoHandoffOutput(uint8_t servoIndex, int16_t currentOutput, int return false; } - if (mixerProfileAT.servoHandoffHoldDurationMs > 0) { - const uint32_t holdElapsedMs = millis() - mixerProfileAT.servoHandoffHoldStartTime; - if (mixerTransitionServoHandoffHoldActive(mixerProfileAT.servoHandoffHoldDurationMs, holdElapsedMs)) { - *output = mixerProfileAT.servoHandoffOutput[servoIndex]; - return true; - } - - mixerProfileAT.servoHandoffHoldDurationMs = 0; - mixerProfileAT.servoHandoffHoldStartTime = 0; - - if (mixerProfileAT.servoHandoffDurationMs == 0) { - clearServoHandoffFade(); - return false; - } - - mixerProfileAT.servoHandoffStartTime = millis(); - } - const float progress = mixerProfileAT.servoHandoffDurationMs == 0 ? 1.0f : constrainf((float)(millis() - mixerProfileAT.servoHandoffStartTime) / (float)mixerProfileAT.servoHandoffDurationMs, 0.0f, 1.0f); diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index dc89b128969..73046913b5e 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -126,6 +126,7 @@ typedef struct mixerProfileATOsdStatus_s { mixerProfileATDirection_e direction; mixerProfileATRequest_e request; mixerProfileATOsdEvent_e event; + mixerProfileATDirection_e switchReminderDirection; } mixerProfileATOsdStatus_t; #endif diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h index e361b547023..5d7ff20a8c3 100644 --- a/src/main/flight/mixer_transition_logic.h +++ b/src/main/flight/mixer_transition_logic.h @@ -100,6 +100,56 @@ static inline bool mixerTransitionCompletedAutoSessionOwnsProfileSwitch( currentProfileIndex != requestedProfileIndex; } +static inline mixerProfileATDirection_e mixerTransitionManualSwitchReminderDirection( + mixerTransitionManualSessionMode_e sessionMode, + bool autoTransitionActive, + bool hotSwitchDone, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex, + bool currentProfileIsMultirotor) +{ + if (sessionMode != MIXER_TRANSITION_MANUAL_SESSION_AUTO || + autoTransitionActive || + !hotSwitchDone) { + return MIXERAT_DIRECTION_NONE; + } + + if (!transitionModeActive && currentProfileIndex == requestedProfileIndex) { + return MIXERAT_DIRECTION_NONE; + } + + return currentProfileIsMultirotor ? MIXERAT_DIRECTION_TO_MC : MIXERAT_DIRECTION_TO_FW; +} + +static inline bool mixerTransitionNavigationHandbackShouldHoldProfile( + bool navigationOwnedProfileSwitchPreviousUpdate, + bool navigationOwnsProfileSwitch, + bool mixerProfileModePresent, + bool autoTransitionActive, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex) +{ + return navigationOwnedProfileSwitchPreviousUpdate && + !navigationOwnsProfileSwitch && + mixerProfileModePresent && + !autoTransitionActive && + (transitionModeActive || currentProfileIndex != requestedProfileIndex); +} + +static inline bool mixerTransitionNavigationHandbackShouldClear( + bool navigationOwnsProfileSwitch, + bool transitionModeRisingEdge, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex) +{ + return navigationOwnsProfileSwitch || + transitionModeRisingEdge || + (!transitionModeActive && currentProfileIndex == requestedProfileIndex); +} + static inline bool mixerTransitionShouldClearCompletedAutoMixingRequest( bool transitionModeActive, bool transitionModeRisingEdge, @@ -236,11 +286,6 @@ static inline uint16_t mixerTransitionComputeServoHandoffDurationMs( return scaleRampTimeMs; } -static inline bool mixerTransitionServoHandoffHoldActive(uint16_t holdDurationMs, uint32_t elapsedMs) -{ - return holdDurationMs > 0 && elapsedMs < holdDurationMs; -} - static inline int16_t mixerTransitionRoundFloatToInt16(float value) { return (int16_t)(value >= 0.0f ? value + 0.5f : value - 0.5f); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3a3f53a2300..acc00dad24c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -6103,9 +6103,10 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter const char *failsafeInfoMessage = NULL; const char *invertedInfoMessage = NULL; + const char *vtolTransitionMessage = NULL; if (ARMING_FLAG(ARMED)) { - const char *vtolTransitionMessage = osdVtolTransitionMessage(); + vtolTransitionMessage = osdVtolTransitionMessage(); if (vtolTransitionMessage) { ADD_MSG(vtolTransitionMessage); } @@ -6363,6 +6364,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // a lost model, but might help avoiding a crash. // Blink to grab user attention. TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (message == vtolTransitionMessage && osdVtolTransitionMessageShouldBlink()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (message == invertedInfoMessage) { TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 692dc2aa35d..7a41bd9d480 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -140,6 +140,8 @@ #define OSD_MSG_VTOL_TRANS_DONE "VTOL TRANSITION DONE" #define OSD_MSG_VTOL_TRANS_ABORTED "VTOL TRANSITION ABORTED" #define OSD_MSG_VTOL_AIRSPEED_TO "VTOL AIRSPEED TIMEOUT" +#define OSD_MSG_VTOL_MOVE_SW_FW "MOVE SWITCH TO FW" +#define OSD_MSG_VTOL_MOVE_SW_MC "MOVE SWITCH TO MC" #ifdef USE_DEV_TOOLS #define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index 539a3502975..9769b5e74c3 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -74,6 +74,8 @@ int16_t osdGetSpeedFromSelectedSource(void) { } #ifdef USE_AUTO_TRANSITION +static bool osdVtolTransitionBlink; + static const char *osdVtolTransitionRequestMessage(const mixerProfileATRequest_e request, const mixerProfileATDirection_e direction) { switch (request) { @@ -117,6 +119,8 @@ static const char *osdVtolTransitionRequestMessage(const mixerProfileATRequest_e const char *osdVtolTransitionMessage(void) { #ifdef USE_AUTO_TRANSITION + osdVtolTransitionBlink = false; + const navVtolTransitionOsdState_e navTransitionState = navigationVtolTransitionOsdState(); switch (navTransitionState) { case NAV_VTOL_TRANSITION_OSD_RETRY_SCAN: @@ -135,6 +139,16 @@ const char *osdVtolTransitionMessage(void) return NULL; } + if (status.switchReminderDirection == MIXERAT_DIRECTION_TO_FW) { + osdVtolTransitionBlink = true; + return OSD_MSG_VTOL_MOVE_SW_FW; + } + + if (status.switchReminderDirection == MIXERAT_DIRECTION_TO_MC) { + osdVtolTransitionBlink = true; + return OSD_MSG_VTOL_MOVE_SW_MC; + } + if (!status.active) { switch (status.event) { case MIXERAT_OSD_EVENT_DONE: @@ -171,6 +185,15 @@ const char *osdVtolTransitionMessage(void) return NULL; } +bool osdVtolTransitionMessageShouldBlink(void) +{ +#ifdef USE_AUTO_TRANSITION + return osdVtolTransitionBlink; +#else + return false; +#endif +} + #endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD) #if defined(USE_OSD) diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h index d162b027e58..1b59539f0f1 100644 --- a/src/main/io/osd_common.h +++ b/src/main/io/osd_common.h @@ -47,6 +47,7 @@ PG_DECLARE(osdCommonConfig_t, osdCommonConfig); int16_t osdGetSpeedFromSelectedSource(void); const char *osdVtolTransitionMessage(void); +bool osdVtolTransitionMessageShouldBlink(void); #endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 27e1704e84d..5333846e464 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -1073,8 +1073,15 @@ static bool djiFormatMessages(char *buff) // Pick one of the available messages. Each message lasts // a second. if (messageCount > 0) { - strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]); - haveMessage = true; + const char *message = messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]; + if (message == vtolTransitionMessage && + osdVtolTransitionMessageShouldBlink() && + OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT / 2, 2) == 0) { + buff[0] = '\0'; + } else { + strcpy(buff, message); + } + haveMessage = true; } #undef ADD_DJI_MSG } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc index 7e71edbe3a2..947c9eab4dd 100644 --- a/src/test/unit/mixer_transition_logic_unittest.cc +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -110,6 +110,160 @@ TEST(MixerTransitionLogicTest, CompletedAutoSessionClearsStaleMixingWhileSwitchR true)); } +TEST(MixerTransitionLogicTest, ManualSwitchReminderShowsAfterCompletedAutoSwitchUntilEndpointConfirmed) +{ + EXPECT_EQ(MIXERAT_DIRECTION_TO_MC, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + true, + true, + 1, + 1, + true)); + + EXPECT_EQ(MIXERAT_DIRECTION_TO_FW, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + true, + false, + 0, + 1, + false)); +} + +TEST(MixerTransitionLogicTest, ManualSwitchReminderClearsWhenEndpointMatches) +{ + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + true, + false, + 1, + 1, + true)); + + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + true, + false, + 0, + 0, + false)); +} + +TEST(MixerTransitionLogicTest, ManualSwitchReminderDoesNotShowForLegacyOrActiveTransition) +{ + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + false, + true, + true, + 1, + 1, + true)); + + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + true, + true, + 1, + 1, + true)); + + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + false, + true, + 1, + 1, + true)); +} + +TEST(MixerTransitionLogicTest, NavigationHandbackHoldsProfileWhenManualSwitchDiffers) +{ + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + false, + true, + false, + false, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldHoldProfile( + false, + false, + true, + false, + false, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + true, + true, + false, + false, + 0, + 1)); +} + +TEST(MixerTransitionLogicTest, NavigationHandbackHoldsProfileWhenSwitchStillInTransitionPosition) +{ + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + false, + true, + false, + true, + 1, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldClear( + false, + false, + true, + 1, + 1)); + + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + false, + false, + false, + 1, + 1)); +} + +TEST(MixerTransitionLogicTest, NavigationHandbackClearsForNewNavigationOrExplicitTransition) +{ + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + true, + false, + false, + 0, + 1)); + + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + false, + true, + true, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + false, + true, + true, + false, + 0, + 1)); +} + TEST(MixerTransitionLogicTest, LegacySessionIgnoresAutoControllerAfterProfileHotSwitch) { mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; @@ -321,14 +475,6 @@ TEST(MixerTransitionLogicTest, ServoHandoffUsesConfiguredScaleRampWhenDynamicMix EXPECT_EQ(0, mixerTransitionComputeServoHandoffDurationMs(false, 0, 750)); } -TEST(MixerTransitionLogicTest, DirectSwitchServoHoldIsIndependentFromHandoffDuration) -{ - EXPECT_TRUE(mixerTransitionServoHandoffHoldActive(300, 0)); - EXPECT_TRUE(mixerTransitionServoHandoffHoldActive(300, 299)); - EXPECT_FALSE(mixerTransitionServoHandoffHoldActive(300, 300)); - EXPECT_FALSE(mixerTransitionServoHandoffHoldActive(0, 0)); -} - TEST(MixerTransitionLogicTest, ServoHandoffBlendStartsFromCapturedOutputAfterHotSwitch) { EXPECT_EQ(1366, mixerTransitionBlendCapturedServoOutput(1366, 980, 0.0f)); From e4915a0c2f2c88868a9b8dd62e995f00dc03921e Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Fri, 26 Jun 2026 12:46:59 +0300 Subject: [PATCH 42/42] SITL build fix --- src/main/common/log.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/common/log.c b/src/main/common/log.c index c2cc30a3a48..36ecaf74ba5 100644 --- a/src/main/common/log.c +++ b/src/main/common/log.c @@ -205,9 +205,11 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t { // Print lines of up to maxBytes bytes. We need 5 characters per byte // 0xAB[space|\n] - const size_t charsPerByte = 5; - const size_t maxBytes = 8; - char buf[LOG_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator + enum { + LOG_BUFFER_HEX_CHARS_PER_BYTE = 5, + LOG_BUFFER_HEX_MAX_BYTES = 8, + }; + char buf[LOG_PREFIX_FORMATTED_SIZE + LOG_BUFFER_HEX_CHARS_PER_BYTE * LOG_BUFFER_HEX_MAX_BYTES + 1]; // +1 for the null terminator size_t bufPos = LOG_PREFIX_FORMATTED_SIZE; const uint8_t *inputPtr = buffer; @@ -219,7 +221,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t for (size_t ii = 0; ii < size; ii++) { tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]); - bufPos += charsPerByte; + bufPos += LOG_BUFFER_HEX_CHARS_PER_BYTE; if (bufPos == sizeof(buf)-1) { buf[bufPos-1] = '\n'; buf[bufPos] = '\0';