diff --git a/docs/VTOL.md b/docs/VTOL.md index 9ef35eba004..ae42de1384a 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -407,6 +407,10 @@ With this global option enabled, an armed manual switch from one VTOL mixer prof Leave `vtol_autotransition_always = OFF` if you want the pilot to keep the explicit choice between a direct profile switch and a separate `MIXER TRANSITION` request. +After leaving a mission, RTH, or automatic landing while armed, the physical switch may disagree with the actual mixer profile. INAV keeps the actual profile until you move the switch to its matching endpoint with `MIXER TRANSITION` OFF. Passing through the middle position does not start a transition or confirm the switch, even if the profile bit happens to match there. This also applies if you interrupt an automatic transition: match the profile that is actually active, not the one the transition intended to reach. Output completion still in progress does not bypass this protection. + +Once the endpoint matches, subsequent switch movements work normally, including `vtol_autotransition_always = ON`. Starting another mission/RTH gives control back to navigation; disarming releases the switch-matching requirement for preflight checks. Active Modes reports the actual mixer profile and actual transition/mixing activity, not simply the physical switch position. + Optional two-position layout with instant FW -> MC: Some pilots want an automated MC -> FW transition, but want FW -> MC to switch almost immediately when they request multicopter mode. This can still be done with `vtol_autotransition_always = ON` by making the FW -> MC transition complete by timer with a zero timer. @@ -519,6 +523,68 @@ It can be used in two contexts: 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. +### Early FW -> MC start during RTH + +When RTH is allowed to land and the armed aircraft is approaching the active Home or SafeHome in FW mode, INAV can start FW -> MC before the aircraft reaches the landing point. No additional radius setting is required. + +INAV estimates the distance needed from information that is already available: + +1. It projects the current trusted GPS ground velocity toward Home. This produces the **closing speed**: only the part of the ground movement that is actually reducing the remaining distance. A crosswind therefore does not incorrectly count all sideways speed as approach speed. +2. It builds a time budget from the FW/source profile. `mixer_switch_trans_timer` contributes its value in `100ms` units. If `mixer_vtol_transition_dynamic_mixer = ON`, `mixer_vtol_transition_scale_ramp_time_ms` is added because the target MC outputs can still be moving after the profile switch. +3. It multiplies closing speed by that time budget. For example, `mixer_switch_trans_timer = 50`, `mixer_vtol_transition_scale_ramp_time_ms = 1200`, and dynamic scaling ON produce a `6.2s` planning budget. At a closing speed of `20m/s`, the predicted start distance is about `124m`. +4. The result is never smaller than `nav_fw_loiter_radius`. If ground velocity is not trusted, the aircraft is moving sideways, or it is moving away from Home, `nav_fw_loiter_radius` becomes the conservative staging distance. +5. The aircraft must remain inside the calculated start distance for `300ms` before INAV starts the transition. This prevents one noisy GPS velocity sample from starting FW -> MC far from Home. + +For the example above, with the global `nav_fw_loiter_radius = 7500` (`75m`): + +| Ground movement toward Home | Time budget | Start boundary | +| --- | --- | --- | +| `20m/s` | `6.2s` | `124m` | +| `10m/s`, for example with more headwind | `6.2s` | `75m`, because the calculated `62m` is inside the staging boundary | +| `25m/s`, for example with more tailwind | `6.2s` | `155m` | +| No trusted velocity, purely sideways movement, or movement away from Home | Staging fallback | `75m` | + +The boundary is recalculated during the approach; the extra `300ms` confirmation means the actual start can be slightly inside it. Ground velocity already reflects the aircraft's movement in the prevailing wind, so this calculation does not require a separate wind estimate. It cannot predict a later gust. + +The mixer timers above belong to the **FW/source mixer profile**. A longer timer or a longer enabled output ramp moves the calculated boundary farther out; a shorter value moves it closer, down to the loiter-radius boundary. With dynamic scaling OFF, the output ramp is excluded from the calculation. With both times at zero, only `nav_fw_loiter_radius` is used. Setting that radius to zero also removes the minimum staging distance; the original arrival/landing path remains available if the early trigger has not fired. + +For an airspeed-controlled transition, this time budget is only a planning estimate. The configured backup timer does not measure how long this airframe actually needs to slow down. In particular, a zero backup timer with a valid pitot does not imply an immediate transition: the airspeed condition can still take several seconds to satisfy. The estimate does not include the later time needed to stop and turn in MC, and cannot guarantee that the aircraft will stop before Home. + +Ground speed only decides **when the procedure starts**. It does not bypass the configured transition safety condition. If `vtol_transition_to_mc_max_airspeed_cm_s` is non-zero and a usable real or virtual pitot source is available, airspeed still decides when the FW -> MC profile switch is safe. If no usable airspeed source is available, the normal `mixer_switch_trans_timer` fallback completes the transition. `mixer_vtol_transition_airspeed_timeout_ms` remains a failure timeout and is not treated as expected transition time. + +This early start is used only for an RTH that is allowed to land. It does not change mission waypoint transitions, manual transitions, RTH configured to loiter without landing, or the separate low-speed FW -> MC safety protection. The source FW profile must have `mixer_automated_switch = ON`, and the paired target profile must be a supported MC profile. + +It runs on the direct Home/SafeHome approach after the RTH climb/trackback stages. While following geozone avoidance waypoints, the early trigger is suspended. Loss of usable position, a route interruption, or a NAV state change clears its confirmation timer. Returning to the approach requires a fresh confirmation. + +Failsafe RTH uses the same approach when landing is allowed. `nav_rth_fs_landing_delay` still delays landing at Home; the earlier profile change can already have happened before this delay starts. Existing transition failure actions still apply if FW -> MC fails. Tailsitter autonomous-transition restrictions also remain in force. + +An RTH-owned transition, including its retry wait/scan, is still part of the same RTH procedure even when the OSD shows altitude hold during the transition. An already selected SafeHome is not replaced with the arming point just because the transition starts. The established RTH altitude plan is retained too: EXTRA altitude is not added again, and the linear-descent plan is not recalculated as though RTH had been switched off. Normal SafeHome permissions and trackback restrictions still apply. + +A brief loss of the position estimate does not by itself deselect an RTH-owned transition. If the existing navigation position-failure timeout expires, or the heading estimate becomes unavailable, INAV aborts the transition and enters its existing emergency-landing procedure. A disabled position-failure timeout keeps its existing meaning; this change adds no new timeout. Pilot cancellation and higher-priority emergency requests still take precedence. + +RC `HOME RESET` is blocked during navigation-owned RTH, mission and LAND transitions, including retry, just as it is blocked during normal RTH/mission flight. This does not block Home reset solely because a manual transition is active, and does not change the separate rules for changing Home through other interfaces. + +### RTH behavior after FW -> MC + +An airplane can pass Home or SafeHome while FW -> MC is still completing. INAV does not immediately command the new MC profile to fly backwards or sideways toward the landing point. + +After a navigation-owned FW -> MC switch during the RTH approach, INAV now performs these steps: + +1. It commands zero climb/descent and keeps the post-switch heading while the MC controller removes the remaining fixed-wing ground speed. The altitude target follows the current altitude during this step to avoid chasing a pre-transition height. The XY target follows the current position, so the aircraft is not pulled back toward the point where the profile switch happened. +2. Once horizontal/vertical speed and roll/pitch remain stable, INAV holds that position and turns the nose toward Home. If already inside the stricter VTOL landing capture radius, it uses the configured Home landing heading instead. +3. The heading and settle conditions must then remain acceptable continuously before normal RTH approach resumes. +4. The aircraft approaches Home nose-first, enters the existing landing settle gate, and only then starts descent. + +During alignment, the direction toward Home is updated from the actual position. A substantial increase in horizontal speed or excessive roll/pitch returns the sequence to braking before attempting another turn. The existing RTH position-sensor timeout and `nav_rth_abort_threshold` checks remain active. There is no timer that forces the aircraft to approach Home while it still fails the settle/heading conditions. + +The braking/alignment stages pause ordinary RTH altitude targeting, including EXTRA climb or linear descent. After they finish, normal RTH altitude management resumes on the approach to Home. If the RTH safe altitude has not yet been reached, it may command a climb again. Changing to another navigation mode clears the old braking/alignment target. + +After RTH selects MC for the landing approach, it stays in MC for that RTH procedure rather than requesting FW again just because Home is still far away. Leaving RTH clears this normal landing choice: for example, RTH -> POSH -> mission can request FW again at a waypoint. This is separate from a genuine low-speed safety switch, whose existing protection continues to prevent automatic FW requests until navigation returns to idle. Changing to POSH alone does not clear that safety protection. + +This sequence uses the existing VTOL settle limits and does not add another CLI setting. It is applied after the successful navigation-owned profile switch even when `vtol_mc_protection_mode = OFF`. When VTOL MC protection is `NAV` or `NAV_AND_STABILIZED`, its throttle reserve, altitude anti-windup bounds, and bailout protection remain active throughout the MC braking, alignment, approach, and landing states. + +The early prediction is intended to reduce overshoot. This post-switch sequence handles an inaccurate prediction by stopping and facing Home before the return approach. Validate the chosen timer values and the resulting stopping distance on the actual airframe; these calculations are not a model of its braking performance. + ## 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. diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 3bd9de242a8..e18b1bb74be 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -1271,7 +1271,6 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) navigationProfileSwitchWasOwned, navigationOwnsProfileSwitch, mixerProfileModePresent, - mixerAT_inuse, transitionModeActive, currentMixerProfileIndex, requestedProfileIndex); diff --git a/src/main/flight/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h index 283d68df75a..633c80bc7a3 100644 --- a/src/main/flight/mixer_transition_logic.h +++ b/src/main/flight/mixer_transition_logic.h @@ -239,15 +239,15 @@ static inline bool mixerTransitionNavigationHandbackShouldHoldProfile( bool navigationOwnedProfileSwitchPreviousUpdate, bool navigationOwnsProfileSwitch, bool mixerProfileModePresent, - bool autoTransitionActive, bool transitionModeActive, int currentProfileIndex, int requestedProfileIndex) { + // Capture the ownership edge even if transition outputs are still finishing. + // Waiting for an idle transition would lose this edge on the next update. return navigationOwnedProfileSwitchPreviousUpdate && !navigationOwnsProfileSwitch && mixerProfileModePresent && - !autoTransitionActive && (transitionModeActive || currentProfileIndex != requestedProfileIndex); } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 7d7cf1629fb..3e46b29ce40 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -328,6 +328,20 @@ typedef struct navMixerATMissionCapture_s { vtolMcProtectionSettleState_t settle; } navMixerATMissionCapture_t; +typedef enum { + NAV_MIXERAT_RTH_CAPTURE_IDLE = 0, + NAV_MIXERAT_RTH_CAPTURE_STOP, + NAV_MIXERAT_RTH_CAPTURE_ALIGN, +} navMixerATRthCaptureStage_e; + +typedef struct navMixerATRthCapture_s { + navMixerATRthCaptureStage_e stage; + vtolMcProtectionSettleState_t settle; + vtolMcProtectionSettleState_t earlyTransitionTrigger; + fpVector3_t holdPos; + int32_t heading; +} navMixerATRthCapture_t; + typedef enum { NAV_MIXERAT_RETRY_STAGE_IDLE = 0, NAV_MIXERAT_RETRY_STAGE_SCAN, @@ -352,8 +366,10 @@ typedef enum { static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; static mixerProfileATRequest_e navMixerATRequestOverride = MIXERAT_REQUEST_NONE; static bool navVtolFwToMcProtectionLatched; +static bool navRthLandingMcSelected; static navMixerATMissionTransition_t navMixerATMissionTransition; static navMixerATMissionCapture_t navMixerATMissionCapture; +static navMixerATRthCapture_t navMixerATRthCapture; #else static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; #endif @@ -391,6 +407,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void); static bool isWaypointMissionValid(void); #ifdef USE_AUTO_TRANSITION static void clearMissionVTOLTransitionState(void); +static navVtolMixerATMode_e navMixerATOwnerMode(void); static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const navWaypoint_t *waypoint); static void updateMissionTransitionGuidance(void); static bool isTransitionRetryToFixedWingRequest(const mixerProfileATRequest_e request); @@ -453,6 +470,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState); #ifdef USE_AUTO_TRANSITION static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_MISSION_CAPTURE(navigationFSMState_t previousState); +static bool updateRthVTOLTransitionCapture(void); +static bool beginRthFwToMcLandingTransitionIfDue(const fpVector3_t *homePos); #endif #ifdef USE_AUTO_TRANSITION static bool beginNavigationFwToMcProtectionTransition(void); @@ -1418,6 +1437,12 @@ static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) #ifdef USE_AUTO_TRANSITION const bool mixerATState = (state == NAV_STATE_MIXERAT_INITIALIZE || state == NAV_STATE_MIXERAT_IN_PROGRESS); + if (state == NAV_STATE_RTH_HEAD_HOME && navMixerATRthCapture.stage != NAV_MIXERAT_RTH_CAPTURE_IDLE) { + // Let altitude protection recognize braking/alignment as a hold. RTH + // still owns XY, so the generic POSH capture cannot replace its target. + stateFlags |= NAV_CTL_HOLD; + } + // During mission-authorized MC->FW transition, command heading only. The // pusher/tilt transition should build speed without the MC position // controller saturating pitch/roll toward a far-away target. @@ -1450,6 +1475,16 @@ navigationFSMStateFlags_t navGetCurrentStateFlags(void) return navGetStateFlags(posControl.navState); } +static bool navIsRthProcedureActive(void) +{ + const bool rthStateActive = (navGetCurrentStateFlags() & NAV_AUTO_RTH) != 0; +#ifdef USE_AUTO_TRANSITION + return navVtolRthProcedureActive(rthStateActive, navMixerATOwnerMode()); +#else + return rthStateActive; +#endif +} + static bool navTerrainFollowingRequested(void) { // Terrain following not supported on FIXED WING aircraft yet @@ -1465,6 +1500,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState navMixerATPendingState = NAV_STATE_IDLE; navMixerATRequestOverride = MIXERAT_REQUEST_NONE; navVtolFwToMcProtectionLatched = false; + navRthLandingMcSelected = false; clearMissionVTOLTransitionState(); #endif resetAltitudeController(false); @@ -1870,13 +1906,28 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio { UNUSED(previousState); - rthAltControlStickOverrideCheck(PITCH); - /* If position sensors unavailable - land immediately */ - if ((posControl.flags.estHeadingStatus == EST_NONE) || !validateRTHSanityChecker()) { + if (posControl.flags.estHeadingStatus == EST_NONE || !validateRTHSanityChecker()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } +#ifdef USE_AUTO_TRANSITION + if (posControl.flags.estPosStatus < EST_USABLE) { + navMixerATRthCapture.earlyTransitionTrigger = (vtolMcProtectionSettleState_t){0}; + } + if (navMixerATRthCapture.stage != NAV_MIXERAT_RTH_CAPTURE_IDLE) { + if (checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + + if (updateRthVTOLTransitionCapture()) { + return NAV_FSM_EVENT_NONE; + } + } +#endif + + rthAltControlStickOverrideCheck(PITCH); + #ifdef USE_AUTO_TRANSITION if (beginNavigationFwToMcProtectionTransition()) { return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; @@ -1884,7 +1935,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio #endif #ifdef USE_AUTO_TRANSITION - const bool allowRthMcToFwTransition = !navVtolFwToMcProtectionLatched; + const bool allowRthMcToFwTransition = navVtolAutomaticFwTransitionAllowed( + navVtolFwToMcProtectionLatched, navRthLandingMcSelected, NAV_VTOL_MIXERAT_MODE_RTH); #else const bool allowRthMcToFwTransition = true; #endif @@ -1911,6 +1963,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio #ifdef USE_GEOZONE // Check for NFZ in our way int8_t wpCount = geozoneCheckForNFZAtCourse(true); +#ifdef USE_AUTO_TRANSITION + if (wpCount != 0 || geozone.avoidInRTHInProgress) { + navMixerATRthCapture.earlyTransitionTrigger = (vtolMcProtectionSettleState_t){0}; + } +#endif if (wpCount > 0) { calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint()); return NAV_FSM_EVENT_NONE; @@ -1940,6 +1997,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio #endif fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); +#ifdef USE_AUTO_TRANSITION + if (beginRthFwToMcLandingTransitionIfDue(tmpHomePos)) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { // Successfully reached position target - update XYZ-position setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); @@ -2502,6 +2565,11 @@ static void clearMissionVTOLTransitionState(void) navMixerATMissionCapture.active = false; navMixerATMissionCapture.settle.stableSinceMs = 0; navMixerATMissionCapture.settle.elapsedMs = 0; + navMixerATRthCapture.stage = NAV_MIXERAT_RTH_CAPTURE_IDLE; + navMixerATRthCapture.settle.stableSinceMs = 0; + navMixerATRthCapture.settle.elapsedMs = 0; + navMixerATRthCapture.earlyTransitionTrigger.stableSinceMs = 0; + navMixerATRthCapture.earlyTransitionTrigger.elapsedMs = 0; } static void startMissionVTOLTransitionCapture(void) @@ -2520,7 +2588,7 @@ static uint16_t missionVTOLTransitionHorizontalSettleLimitCmS(void) #endif } -static bool missionVTOLTransitionCaptureIsReady(void) +static bool vtolTransitionCaptureIsReady(vtolMcProtectionSettleState_t *settle) { const bool horizontalVelocityUsable = posControl.flags.estVelStatus == EST_TRUSTED; const bool verticalVelocityUsable = posControl.flags.estAltStatus >= EST_USABLE; @@ -2536,12 +2604,146 @@ static bool missionVTOLTransitionCaptureIsReady(void) vtolMcProtectionSettleAttitudeLimitDeciDeg(navConfig()->mc.max_bank_angle)); return vtolMcProtectionUpdateSettleState( - &navMixerATMissionCapture.settle, + settle, conditionsMet, vtolMcProtectionSettleTimeMs(horizontalVelocityUsable), millis()); } +static void startRthVTOLTransitionCapture(void) +{ + navMixerATRthCapture.stage = NAV_MIXERAT_RTH_CAPTURE_STOP; + navMixerATRthCapture.settle.stableSinceMs = 0; + navMixerATRthCapture.settle.elapsedMs = 0; + navMixerATRthCapture.holdPos = navGetCurrentActualPositionAndVelocity()->pos; + navMixerATRthCapture.heading = posControl.actualState.yaw; + initializeRTHSanityChecker(); + + // Once RTH has selected MC for the landing approach, do not let the normal + // long-distance RTH optimization immediately request MC->FW again. + navRthLandingMcSelected = true; +} + +static bool beginRthFwToMcLandingTransitionIfDue(const fpVector3_t *homePos) +{ + const bool transitionAvailable = + ARMING_FLAG(ARMED) && + navigationRTHAllowsLanding() && + STATE(FIXED_WING_LEGACY) && + !mixerATIsActive() && + checkMixerATRequired(MIXERAT_REQUEST_LAND); + + if (!transitionAvailable) { + vtolMcProtectionUpdateSettleState( + &navMixerATRthCapture.earlyTransitionTrigger, + false, + VTOL_MC_RTH_TRANSITION_TRIGGER_CONFIRM_MS, + millis()); + return false; + } + + const navEstimatedPosVel_t *actual = navGetCurrentActualPositionAndVelocity(); + const uint32_t closingSpeedCmS = vtolMcProtectionRthClosingSpeedCmS( + posControl.flags.estVelStatus == EST_TRUSTED, + homePos->x - actual->pos.x, + homePos->y - actual->pos.y, + actual->vel.x, + actual->vel.y); + + const uint32_t transitionTimeBudgetMs = vtolMcProtectionRthTransitionTimeBudgetMs( + currentMixerConfig.switchTransitionTimer, + currentMixerConfig.vtolTransitionDynamicMixer, + currentMixerConfig.vtolTransitionScaleRampTimeMs); + const uint32_t startDistanceCm = vtolMcProtectionRthTransitionStartDistanceCm( + closingSpeedCmS, + transitionTimeBudgetMs, + navConfig()->fw.loiter_radius); + const bool insideStartDistance = calculateDistanceToDestination(homePos) <= startDistanceCm; + + if (!vtolMcProtectionUpdateSettleState( + &navMixerATRthCapture.earlyTransitionTrigger, + insideStartDistance, + VTOL_MC_RTH_TRANSITION_TRIGGER_CONFIRM_MS, + millis())) { + return false; + } + + navMixerATRequestOverride = MIXERAT_REQUEST_LAND; + return true; +} + +static bool updateRthVTOLTransitionCapture(void) +{ + const fpVector3_t actualPos = navGetCurrentActualPositionAndVelocity()->pos; + + if (!ARMING_FLAG(ARMED) || !STATE(MULTIROTOR)) { + navMixerATRthCapture.stage = NAV_MIXERAT_RTH_CAPTURE_IDLE; + return false; + } + + if (navMixerATRthCapture.stage == NAV_MIXERAT_RTH_CAPTURE_ALIGN && + vtolMcProtectionLandingDescentNeedsResettle( + true, true, + posControl.flags.estVelStatus == EST_TRUSTED, + posControl.actualState.velXY, + MAX(ABS(attitude.values.roll), ABS(attitude.values.pitch)), + missionVTOLTransitionHorizontalSettleLimitCmS(), + vtolMcProtectionSettleAttitudeLimitDeciDeg(navConfig()->mc.max_bank_angle))) { + // A gust during alignment requires braking again before another turn. + navMixerATRthCapture.stage = NAV_MIXERAT_RTH_CAPTURE_STOP; + navMixerATRthCapture.settle = (vtolMcProtectionSettleState_t){0}; + navMixerATRthCapture.heading = posControl.actualState.yaw; + } + + if (navMixerATRthCapture.stage == NAV_MIXERAT_RTH_CAPTURE_STOP) { + // Remove the remaining fixed-wing velocity without pulling back toward + // Home. Keep altitude and the post-switch heading while MC settles. + setDesiredPosition(&actualPos, + navMixerATRthCapture.heading, + NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + + if (!vtolTransitionCaptureIsReady(&navMixerATRthCapture.settle)) { + return true; + } + + navMixerATRthCapture.stage = NAV_MIXERAT_RTH_CAPTURE_ALIGN; + navMixerATRthCapture.settle.stableSinceMs = 0; + navMixerATRthCapture.settle.elapsedMs = 0; + navMixerATRthCapture.holdPos = actualPos; + } + + // Re-evaluate against the real position: drift during the turn must not + // authorize an approach using a heading that now points away from Home. + const fpVector3_t *homePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); + navMixerATRthCapture.heading = vtolMcProtectionRthPostSwitchHeading( + calculateDistanceToDestination(homePos), + vtolMcProtectionLandingCaptureRadiusCm(navConfig()->general.waypoint_radius), + calculateBearingToDestination(homePos), + posControl.rthState.homePosition.heading); + setDesiredPosition(&navMixerATRthCapture.holdPos, + navMixerATRthCapture.heading, + NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + + const bool headingReached = ABS(wrap_18000(navMixerATRthCapture.heading - posControl.actualState.yaw)) <= + VTOL_MC_RTH_APPROACH_HEADING_TOLERANCE_CD; + if (!headingReached) { + navMixerATRthCapture.settle.stableSinceMs = 0; + navMixerATRthCapture.settle.elapsedMs = 0; + return true; + } + + if (!vtolTransitionCaptureIsReady(&navMixerATRthCapture.settle)) { + return true; + } + + navMixerATRthCapture.stage = NAV_MIXERAT_RTH_CAPTURE_IDLE; + posControl.activeWaypoint.bearing = calculateBearingToDestination(&posControl.rthState.homePosition.pos); + initializeRTHSanityChecker(); + return false; +} + static void rebaseMissionWaypointAfterTransition(void) { const fpVector3_t actualPosition = navGetCurrentActualPositionAndVelocity()->pos; @@ -2832,7 +3034,8 @@ static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const // 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) { + if (transitionToFixedWing && !navVtolAutomaticFwTransitionAllowed( + navVtolFwToMcProtectionLatched, navRthLandingMcSelected, NAV_VTOL_MIXERAT_MODE_WAYPOINT)) { return NAV_MISSION_VTOL_TRANSITION_CONTINUE; } @@ -3511,6 +3714,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav return NAV_FSM_EVENT_SWITCH_TO_IDLE; } + // Keep brief position-estimate losses inside RTH, but retain its normal + // timeout/heading-failure escape rather than leaving a transition orphaned. + if (navVtolRthTransitionHasSensorFailure( + navMixerATOwnerMode(), + posControl.flags.estHeadingStatus != EST_NONE, + checkForPositionSensorTimeout())) { + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + clearMissionVTOLTransitionState(); + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + if (navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE) { const navMixerATRetryScanResult_e retryResult = updateMissionTransitionRetryScan(); if (retryResult == NAV_MIXERAT_RETRY_SCAN_READY_TO_RETRY) { @@ -3562,6 +3777,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav navigationFSMEvent_t nextEvent = NAV_FSM_EVENT_SWITCH_TO_IDLE; bool startMissionCapture = false; + bool startRthCapture = false; bool rebaseMissionWaypoint = false; if (transitionAborted) { nextEvent = getTransitionFailEvent(required_action); @@ -3581,6 +3797,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav case NAV_STATE_RTH_HEAD_HOME: case NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING: nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + startRthCapture = STATE(MULTIROTOR); break; case NAV_STATE_RTH_LOITER_ABOVE_HOME: nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH; @@ -3599,9 +3816,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav { case NAV_STATE_RTH_HEAD_HOME: nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + startRthCapture = required_action == MIXERAT_REQUEST_LAND && STATE(MULTIROTOR); break; case NAV_STATE_RTH_LANDING: - nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + if (STATE(MULTIROTOR)) { + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + startRthCapture = true; + } else { + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + } break; case NAV_STATE_WAYPOINT_RTH_LAND: nextEvent = NAV_FSM_EVENT_SWITCH_TO_LANDING; @@ -3637,6 +3860,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav clearMissionVTOLTransitionState(); if (startMissionCapture) { startMissionVTOLTransitionCapture(); + } else if (startRthCapture) { + startRthVTOLTransitionCapture(); } else if (rebaseMissionWaypoint) { rebaseMissionWaypointAfterTransition(); } @@ -3730,7 +3955,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_MISSION_CAPTURE posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - if (!missionVTOLTransitionCaptureIsReady()) { + if (!vtolTransitionCaptureIsReady(&navMixerATMissionCapture.settle)) { return NAV_FSM_EVENT_NONE; } @@ -4042,6 +4267,29 @@ static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) previousState = posControl.navState; if (posControl.navState != newState) { +#ifdef USE_AUTO_TRANSITION + navVtolMixerATMode_e nextTransitionOwner = NAV_VTOL_MIXERAT_MODE_NONE; + if (newState == NAV_STATE_MIXERAT_INITIALIZE) { + // onEntry has not recorded navMixerATPendingState yet. + if (navGetStateFlags(previousState) & NAV_AUTO_RTH) { + nextTransitionOwner = NAV_VTOL_MIXERAT_MODE_RTH; + } + } else if (newState == NAV_STATE_MIXERAT_IN_PROGRESS) { + nextTransitionOwner = navMixerATOwnerMode(); + } + // Clear at the state boundary, not just at the end of the FSM pass: + // another mode can fail back into RTH within this same pass. + navRthLandingMcSelected = navVtolRthLandingMcSelectionRetained( + navRthLandingMcSelected, + ARMING_FLAG(ARMED), + (navGetStateFlags(newState) & NAV_AUTO_RTH) != 0, + nextTransitionOwner); + navMixerATRthCapture.earlyTransitionTrigger = (vtolMcProtectionSettleState_t){0}; + if (newState != NAV_STATE_RTH_HEAD_HOME) { + navMixerATRthCapture.stage = NAV_MIXERAT_RTH_CAPTURE_IDLE; + navMixerATRthCapture.settle = (vtolMcProtectionSettleState_t){0}; + } +#endif posControl.navState = newState; posControl.navPersistentId = navFSM[newState].persistentId; } @@ -4090,6 +4338,10 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) } #ifdef USE_AUTO_TRANSITION + if (!ARMING_FLAG(ARMED)) { + navRthLandingMcSelected = false; + } + if (navMixerATMissionCapture.active && posControl.navState != NAV_STATE_MIXERAT_MISSION_CAPTURE) { // A mode/failsafe change interrupted capture. Never leak it into a // later mission or navigation session. @@ -4530,7 +4782,7 @@ float getFinalRTHAltitude(void) static void updateDesiredRTHAltitude(void) { if (ARMING_FLAG(ARMED)) { - if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH) + if (!(navIsRthProcedureActive() || ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) { switch (navConfig()->general.flags.rth_climb_first_stage_mode) { case NAV_RTH_CLIMB_STAGE_AT_LEAST: @@ -4800,7 +5052,12 @@ void updateHomePosition(void) static bool isHomeResetAllowed = false; // If pilot so desires he may reset home position to current position if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { - if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { +#ifdef USE_AUTO_TRANSITION + const bool transitionBlocksHomeReset = navVtolTransitionBlocksHomeReset(navMixerATOwnerMode()); +#else + const bool transitionBlocksHomeReset = false; +#endif + if (isHomeResetAllowed && !transitionBlocksHomeReset && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { homeUpdateFlags = 0; homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setHome = true; @@ -6031,7 +6288,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) const bool canActivateAltHold = canActivateAltHoldMode(); const bool canActivatePosHold = canActivatePosHoldMode(); const bool canActivateNavigation = canActivateNavigationModes(); - const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH; + const bool isExecutingRTH = navIsRthProcedureActive(); #ifdef USE_AUTO_TRANSITION const navigationFSMEvent_t orphanedTransitionEvent = abortOrphanedNavigationMixerATTransition(); if (orphanedTransitionEvent != NAV_FSM_EVENT_NONE) { @@ -6117,7 +6374,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) * This might switch to emergency landing controller if GPS is unavailable */ if (posControl.flags.forcedRTHActivated) { #ifdef USE_AUTO_TRANSITION - if (navMixerATOwnsMode(NAV_VTOL_MIXERAT_MODE_RTH)) { + if (navMixerATOwnerMode() == NAV_VTOL_MIXERAT_MODE_RTH) { return NAV_FSM_EVENT_NONE; } #endif @@ -6183,7 +6440,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) { if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { #ifdef USE_AUTO_TRANSITION - if (navMixerATOwnsMode(NAV_VTOL_MIXERAT_MODE_RTH)) { + if (navMixerATOwnerMode() == NAV_VTOL_MIXERAT_MODE_RTH) { return NAV_FSM_EVENT_NONE; } #endif @@ -6637,6 +6894,7 @@ void navigationInit(void) navMixerATPendingState = NAV_STATE_IDLE; navMixerATRequestOverride = MIXERAT_REQUEST_NONE; navVtolFwToMcProtectionLatched = false; + navRthLandingMcSelected = false; clearMissionVTOLTransitionState(); #endif diff --git a/src/main/navigation/navigation_vtol_mc_protection_logic.h b/src/main/navigation/navigation_vtol_mc_protection_logic.h index 89ca69ee04f..12147c905d4 100644 --- a/src/main/navigation/navigation_vtol_mc_protection_logic.h +++ b/src/main/navigation/navigation_vtol_mc_protection_logic.h @@ -19,10 +19,13 @@ #include #include +#include #include "common/time.h" #define VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM 100 +#define VTOL_MC_RTH_APPROACH_HEADING_TOLERANCE_CD 1500 +#define VTOL_MC_RTH_TRANSITION_TRIGGER_CONFIRM_MS 300 #define VTOL_MC_LANDING_RESETTLE_SPEED_MULTIPLIER 2 #define VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S 100 #define VTOL_MC_SETTLE_ATTITUDE_CAP_DEG 20 @@ -71,6 +74,68 @@ static inline uint16_t vtolMcProtectionLandingCaptureRadiusCm(const uint16_t nav return navWpRadiusCm < VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM ? navWpRadiusCm : VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM; } +static inline int32_t vtolMcProtectionRthPostSwitchHeading( + const uint32_t distanceToHomeCm, + const uint16_t landingCaptureRadiusCm, + const int32_t bearingToHomeCd, + const int32_t landingHeadingCd) +{ + // Close to Home the bearing becomes noisy, so use the configured landing + // heading. Farther away, face Home before allowing the approach to resume. + return distanceToHomeCm > landingCaptureRadiusCm ? bearingToHomeCd : landingHeadingCd; +} + +static inline uint32_t vtolMcProtectionRthTransitionTimeBudgetMs( + const int16_t switchTransitionTimerDeciseconds, + const bool dynamicMixerEnabled, + const uint16_t scaleRampTimeMs) +{ + const uint32_t switchTimeMs = switchTransitionTimerDeciseconds > 0 ? + (uint32_t)switchTransitionTimerDeciseconds * 100U : 0U; + const uint32_t outputRampTimeMs = dynamicMixerEnabled ? scaleRampTimeMs : 0U; + + return switchTimeMs + outputRampTimeMs; +} + +static inline uint32_t vtolMcProtectionRthClosingSpeedCmS( + const bool velocityTrusted, + const float deltaX, + const float deltaY, + const float velocityX, + const float velocityY) +{ + if (!velocityTrusted) { + return 0; + } + + const float distance = sqrtf(deltaX * deltaX + deltaY * deltaY); + if (!isfinite(distance) || distance <= 0.0f) { + return 0; + } + + const float closingSpeed = (velocityX * deltaX + velocityY * deltaY) / distance; + // Reject invalid estimates before converting to an integer (lrintf uses a + // signed 32-bit long on flight targets). Receding/tangential flight uses staging. + return isfinite(closingSpeed) && closingSpeed > 0.0f && closingSpeed < (float)INT32_MAX ? + (uint32_t)lrintf(closingSpeed) : 0; +} + +static inline uint32_t vtolMcProtectionRthTransitionStartDistanceCm( + const uint32_t closingSpeedCmS, + const uint32_t transitionTimeBudgetMs, + const uint16_t fallbackRadiusCm) +{ + const uint64_t predictedDistanceCm = + ((uint64_t)closingSpeedCmS * transitionTimeBudgetMs + 999U) / 1000U; + + if (predictedDistanceCm > UINT32_MAX) { + return UINT32_MAX; + } + + const uint32_t predictedDistance = (uint32_t)predictedDistanceCm; + return predictedDistance > fallbackRadiusCm ? predictedDistance : fallbackRadiusCm; +} + static inline uint16_t vtolMcProtectionHorizontalSettleSpeedCmS(const uint16_t brakingDisengageSpeedCmS) { return brakingDisengageSpeedCmS > 0 ? brakingDisengageSpeedCmS : VTOL_MC_DEFAULT_BRAKING_DISENGAGE_CM_S; diff --git a/src/main/navigation/navigation_vtol_mission_logic.h b/src/main/navigation/navigation_vtol_mission_logic.h index c5a20b8e52d..d1de0dcc943 100644 --- a/src/main/navigation/navigation_vtol_mission_logic.h +++ b/src/main/navigation/navigation_vtol_mission_logic.h @@ -41,6 +41,50 @@ typedef enum { NAV_VTOL_MIXERAT_MODE_EMERGENCY_LANDING, } navVtolMixerATMode_e; +// Procedure ownership outlives mixer activity during initialization and retry. +// These checks deliberately do not enable any additional NAV controllers. +static inline bool navVtolRthProcedureActive( + const bool rthStateActive, + const navVtolMixerATMode_e transitionOwner) +{ + return rthStateActive || transitionOwner == NAV_VTOL_MIXERAT_MODE_RTH; +} + +static inline bool navVtolTransitionBlocksHomeReset(const navVtolMixerATMode_e transitionOwner) +{ + return transitionOwner == NAV_VTOL_MIXERAT_MODE_RTH || + transitionOwner == NAV_VTOL_MIXERAT_MODE_WAYPOINT || + transitionOwner == NAV_VTOL_MIXERAT_MODE_LAND; +} + +static inline bool navVtolRthTransitionHasSensorFailure( + const navVtolMixerATMode_e transitionOwner, + const bool headingAvailable, + const bool positionTimedOut) +{ + return transitionOwner == NAV_VTOL_MIXERAT_MODE_RTH && + (!headingAvailable || positionTimedOut); +} + +static inline bool navVtolRthLandingMcSelectionRetained( + const bool selected, + const bool armed, + const bool rthStateActive, + const navVtolMixerATMode_e transitionOwner) +{ + return selected && armed && + (rthStateActive || transitionOwner == NAV_VTOL_MIXERAT_MODE_RTH); +} + +static inline bool navVtolAutomaticFwTransitionAllowed( + const bool lowSpeedProtectionLatched, + const bool rthLandingMcSelected, + const navVtolMixerATMode_e requestingMode) +{ + return !lowSpeedProtectionLatched && + !(rthLandingMcSelected && requestingMode == NAV_VTOL_MIXERAT_MODE_RTH); +} + static inline bool navVtolMixerATModeRequestIsOwned( const bool mixerAtActive, const navVtolMixerATMode_e owner, diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc index 4368658d739..212228f22f1 100644 --- a/src/test/unit/mixer_transition_logic_unittest.cc +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -386,7 +386,6 @@ TEST(MixerTransitionLogicTest, NavigationHandbackHoldsProfileWhenManualSwitchDif false, true, false, - false, 0, 1)); @@ -395,7 +394,6 @@ TEST(MixerTransitionLogicTest, NavigationHandbackHoldsProfileWhenManualSwitchDif false, true, false, - false, 0, 1)); @@ -404,7 +402,6 @@ TEST(MixerTransitionLogicTest, NavigationHandbackHoldsProfileWhenManualSwitchDif true, true, false, - false, 0, 1)); } @@ -415,7 +412,6 @@ TEST(MixerTransitionLogicTest, NavigationHandbackHoldsProfileWhenSwitchStillInTr true, false, true, - false, true, 1, 1)); @@ -450,8 +446,7 @@ TEST(MixerTransitionLogicTest, NavigationHandbackClearsForNewNavigationOrMatchin EXPECT_FALSE(mixerTransitionNavigationHandbackShouldHoldProfile( true, false, - true, - true, + false, false, 0, 1)); @@ -489,7 +484,6 @@ TEST(MixerTransitionLogicTest, NavigationHandbackIgnoresIntermediatePositionUnti true, false, true, - false, true, 0, 1)); @@ -512,7 +506,6 @@ TEST(MixerTransitionLogicTest, NavigationHandbackIgnoresIntermediatePositionUnti true, false, true, - false, true, 1, 0)); diff --git a/src/test/unit/mixer_transition_scenarios_unittest.cc b/src/test/unit/mixer_transition_scenarios_unittest.cc index 95004bca159..21963d821a5 100644 --- a/src/test/unit/mixer_transition_scenarios_unittest.cc +++ b/src/test/unit/mixer_transition_scenarios_unittest.cc @@ -76,6 +76,71 @@ float computeTransitionServoBlendForStep( } // namespace +TEST(MixerTransitionScenarioTest, NavigationHandbackRequiresEndpointMatchBeforeAnotherManualRequest) +{ + for (const bool mission : {false, true}) { + for (const bool always : {false, true}) { + for (const int currentProfile : {0, 1}) { + SCOPED_TRACE(::testing::Message() << "mission=" << mission << " always=" << always + << " currentProfile=" << currentProfile); + bool wasOwned = true; + bool pending = false; + auto update = [&](bool owns, bool middle, int requested) { + const bool hold = pending || mixerTransitionNavigationHandbackShouldHoldProfile( + wasOwned, owns, true, middle, currentProfile, requested); + const bool allowed = mixerTransitionManualInputAllowed(owns, hold); + if (mixerTransitionNavigationHandbackShouldClear(owns, middle, currentProfile, requested)) { + pending = false; + } else if (hold) { + pending = true; + } + wasOwned = owns; + return allowed; + }; + + const bool owns = mixerTransitionNavigationOwnsProfileSwitch( + true, true, mission, !mission, false, false); + EXPECT_FALSE(update(owns, true, 1 - currentProfile)); + EXPECT_FALSE(pending); + + // NAV ends before or after hot-switch. Either actual profile must + // be retained, even while transition/output cleanup is still active. + EXPECT_FALSE(update(false, false, 1 - currentProfile)); + EXPECT_TRUE(pending); + for (const bool transitionActive : {true, false}) { + EXPECT_FALSE(update(false, true, 1 - currentProfile)); + EXPECT_TRUE(pending); + EXPECT_FALSE(mixerTransitionManualMixingRequestMayUpdate( + false, transitionActive, false, pending)); + EXPECT_FALSE(mixerTransitionProfileSwitchShouldStartAutoTransition( + always, true, true, true, true, transitionActive, + false, pending, false, false, currentProfile, 1 - currentProfile)); + } + + // Middle is not a confirmation even if its profile bit matches. + EXPECT_FALSE(update(false, true, currentProfile)); + EXPECT_TRUE(pending); + EXPECT_FALSE(update(false, false, currentProfile)); + EXPECT_FALSE(pending); + EXPECT_TRUE(update(false, false, currentProfile)); + + // Only a subsequent deliberate selection may start a transition + // (always=ON) or use the existing direct-switch policy (OFF). + EXPECT_TRUE(update(false, false, 1 - currentProfile)); + EXPECT_EQ(always, mixerTransitionProfileSwitchShouldStartAutoTransition( + always, true, true, true, true, false, + false, pending, false, false, currentProfile, 1 - currentProfile)); + EXPECT_EQ(!always, mixerTransitionProfileSwitchDirectSwitchAllowed(always, true, true)); + + EXPECT_FALSE(update(true, false, 1 - currentProfile)); + EXPECT_FALSE(pending); + EXPECT_FALSE(update(false, false, 1 - currentProfile)); + EXPECT_TRUE(pending); + } + } + } +} + TEST(MixerTransitionScenarioTest, LegacyManualMcToFwSessionStaysLegacyAcrossProfileHotSwitch) { ManualTransitionScenario scenario(true); diff --git a/src/test/unit/navigation_vtol_mission_logic_unittest.cc b/src/test/unit/navigation_vtol_mission_logic_unittest.cc index b59fe008db8..fcab77e25c5 100644 --- a/src/test/unit/navigation_vtol_mission_logic_unittest.cc +++ b/src/test/unit/navigation_vtol_mission_logic_unittest.cc @@ -4,6 +4,108 @@ extern "C" { #include "navigation/navigation_vtol_mission_logic.h" } +TEST(NavigationVtolMissionLogicTest, RthLandingSelectionSurvivesInternalStatesAndTransition) +{ + EXPECT_TRUE(navVtolRthLandingMcSelectionRetained(true, true, true, NAV_VTOL_MIXERAT_MODE_NONE)); + EXPECT_TRUE(navVtolRthLandingMcSelectionRetained(true, true, false, NAV_VTOL_MIXERAT_MODE_RTH)); + EXPECT_FALSE(navVtolRthLandingMcSelectionRetained(false, true, true, NAV_VTOL_MIXERAT_MODE_NONE)); +} + +TEST(NavigationVtolMissionLogicTest, RthLandingSelectionClearsOnModeExitAndCannotLeakIntoNewRth) +{ + for (const auto nextOwner : {NAV_VTOL_MIXERAT_MODE_NONE, NAV_VTOL_MIXERAT_MODE_POSHOLD, + NAV_VTOL_MIXERAT_MODE_WAYPOINT, NAV_VTOL_MIXERAT_MODE_LAND, + NAV_VTOL_MIXERAT_MODE_EMERGENCY_LANDING}) { + bool selected = navVtolRthLandingMcSelectionRetained(true, true, false, nextOwner); + EXPECT_FALSE(selected); + selected = navVtolRthLandingMcSelectionRetained(selected, true, false, NAV_VTOL_MIXERAT_MODE_WAYPOINT); + EXPECT_FALSE(selected); + EXPECT_FALSE(navVtolRthLandingMcSelectionRetained(selected, true, true, NAV_VTOL_MIXERAT_MODE_NONE)); + } +} + +TEST(NavigationVtolMissionLogicTest, DisarmClearsRthLandingSelectionEvenBeforeNavStateChanges) +{ + EXPECT_FALSE(navVtolRthLandingMcSelectionRetained(true, false, true, NAV_VTOL_MIXERAT_MODE_NONE)); + EXPECT_FALSE(navVtolRthLandingMcSelectionRetained(true, false, false, NAV_VTOL_MIXERAT_MODE_RTH)); +} + +TEST(NavigationVtolMissionLogicTest, NormalRthLandingDoesNotBlockMissionFwButSafetyProtectionStillDoes) +{ + EXPECT_FALSE(navVtolAutomaticFwTransitionAllowed(false, true, NAV_VTOL_MIXERAT_MODE_RTH)); + EXPECT_TRUE(navVtolAutomaticFwTransitionAllowed(false, true, NAV_VTOL_MIXERAT_MODE_WAYPOINT)); + + const bool selectedAfterPoshold = navVtolRthLandingMcSelectionRetained( + true, true, false, NAV_VTOL_MIXERAT_MODE_NONE); + for (const auto mode : {NAV_VTOL_MIXERAT_MODE_RTH, NAV_VTOL_MIXERAT_MODE_WAYPOINT}) { + EXPECT_TRUE(navVtolAutomaticFwTransitionAllowed(false, selectedAfterPoshold, mode)); + // Neither a mode exit nor a simultaneous normal RTH landing selection + // may undo the independent low-speed safety decision. + EXPECT_FALSE(navVtolAutomaticFwTransitionAllowed(true, selectedAfterPoshold, mode)); + EXPECT_FALSE(navVtolAutomaticFwTransitionAllowed(true, true, mode)); + } +} + +TEST(NavigationVtolMissionLogicTest, RthProcedureKeepsContextDuringTransitionAndRetry) +{ + EXPECT_TRUE(navVtolRthProcedureActive(true, NAV_VTOL_MIXERAT_MODE_NONE)); + EXPECT_TRUE(navVtolRthProcedureActive(false, NAV_VTOL_MIXERAT_MODE_RTH)); + + // Retry may have no active mixer transition. RTH context (SafeHome, + // altitude plan and selector ownership) must nevertheless remain intact. + EXPECT_FALSE(navVtolMixerATModeRequestIsOwned( + false, NAV_VTOL_MIXERAT_MODE_RTH, NAV_VTOL_MIXERAT_MODE_RTH)); + EXPECT_TRUE(navVtolRthProcedureActive(false, NAV_VTOL_MIXERAT_MODE_RTH)); +} + +TEST(NavigationVtolMissionLogicTest, NonRthTransitionDoesNotAcquireRthContext) +{ + for (const auto owner : {NAV_VTOL_MIXERAT_MODE_NONE, NAV_VTOL_MIXERAT_MODE_WAYPOINT, + NAV_VTOL_MIXERAT_MODE_LAND, NAV_VTOL_MIXERAT_MODE_POSHOLD, + NAV_VTOL_MIXERAT_MODE_EMERGENCY_LANDING}) { + EXPECT_FALSE(navVtolRthProcedureActive(false, owner)); + } +} + +TEST(NavigationVtolMissionLogicTest, HomeResetRemainsBlockedThroughoutNavigationOwnedTransitions) +{ + for (const auto owner : {NAV_VTOL_MIXERAT_MODE_RTH, NAV_VTOL_MIXERAT_MODE_WAYPOINT, + NAV_VTOL_MIXERAT_MODE_LAND}) { + EXPECT_TRUE(navVtolTransitionBlocksHomeReset(owner)); + } + // No stale pending state may block manual flight after NAV ownership ends. + EXPECT_FALSE(navVtolTransitionBlocksHomeReset(NAV_VTOL_MIXERAT_MODE_NONE)); + EXPECT_FALSE(navVtolTransitionBlocksHomeReset(NAV_VTOL_MIXERAT_MODE_POSHOLD)); + EXPECT_FALSE(navVtolTransitionBlocksHomeReset(NAV_VTOL_MIXERAT_MODE_EMERGENCY_LANDING)); +} + +TEST(NavigationVtolMissionLogicTest, RthTransitionWaitsThroughBriefPositionLossButNotSensorFailure) +{ + EXPECT_FALSE(navVtolRthTransitionHasSensorFailure(NAV_VTOL_MIXERAT_MODE_RTH, true, false)); + EXPECT_TRUE(navVtolRthTransitionHasSensorFailure(NAV_VTOL_MIXERAT_MODE_RTH, true, true)); + EXPECT_TRUE(navVtolRthTransitionHasSensorFailure(NAV_VTOL_MIXERAT_MODE_RTH, false, false)); + EXPECT_TRUE(navVtolRthTransitionHasSensorFailure(NAV_VTOL_MIXERAT_MODE_RTH, false, true)); + // Do not impose RTH's failure policy on a different transition owner. + for (const auto owner : {NAV_VTOL_MIXERAT_MODE_NONE, NAV_VTOL_MIXERAT_MODE_WAYPOINT, + NAV_VTOL_MIXERAT_MODE_LAND}) { + EXPECT_FALSE(navVtolRthTransitionHasSensorFailure(owner, false, true)); + } +} + +TEST(NavigationVtolMissionLogicTest, RthTransitionContextIsReleasedOnExitAndRestoredOnlyByNewRth) +{ + const navVtolMixerATMode_e owners[] = { + NAV_VTOL_MIXERAT_MODE_RTH, NAV_VTOL_MIXERAT_MODE_NONE, + NAV_VTOL_MIXERAT_MODE_WAYPOINT, NAV_VTOL_MIXERAT_MODE_RTH, + }; + const bool expectedRth[] = {true, false, false, true}; + const bool expectedHomeResetBlock[] = {true, false, true, true}; + for (unsigned i = 0; i < 4; i++) { + EXPECT_EQ(expectedRth[i], navVtolRthProcedureActive(false, owners[i])); + EXPECT_EQ(expectedHomeResetBlock[i], navVtolTransitionBlocksHomeReset(owners[i])); + } +} + TEST(NavigationVtolMissionLogicTest, ReadyWhenAllPreconditionsAreMet) { EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_READY, diff --git a/src/test/unit/vtol_mc_protection_logic_unittest.cc b/src/test/unit/vtol_mc_protection_logic_unittest.cc index 14117b11cbe..829fcb8dff0 100644 --- a/src/test/unit/vtol_mc_protection_logic_unittest.cc +++ b/src/test/unit/vtol_mc_protection_logic_unittest.cc @@ -163,6 +163,78 @@ TEST(VtolMcProtectionLogicTest, LandingCaptureRadiusCapsLargeWaypointRadius) EXPECT_EQ(80, vtolMcProtectionLandingCaptureRadiusCm(80)); } +TEST(VtolMcProtectionLogicTest, RthPostSwitchFacesHomeUntilInsideLandingCaptureRadius) +{ + EXPECT_EQ(12345, vtolMcProtectionRthPostSwitchHeading(2500, 100, 12345, 27000)); + EXPECT_EQ(27000, vtolMcProtectionRthPostSwitchHeading(100, 100, 12345, 27000)); + EXPECT_EQ(27000, vtolMcProtectionRthPostSwitchHeading(0, 100, 12345, 27000)); +} + +TEST(VtolMcProtectionLogicTest, RthTransitionTimeBudgetIncludesTimerAndDynamicOutputRamp) +{ + EXPECT_EQ(6200U, vtolMcProtectionRthTransitionTimeBudgetMs(50, true, 1200)); + EXPECT_EQ(5000U, vtolMcProtectionRthTransitionTimeBudgetMs(50, false, 1200)); + EXPECT_EQ(1200U, vtolMcProtectionRthTransitionTimeBudgetMs(0, true, 1200)); + EXPECT_EQ(0U, vtolMcProtectionRthTransitionTimeBudgetMs(-1, false, 1200)); +} + +TEST(VtolMcProtectionLogicTest, RthTransitionStartDistanceUsesClosingSpeedOrLoiterFallback) +{ + EXPECT_EQ(12400U, vtolMcProtectionRthTransitionStartDistanceCm(2000, 6200, 7500)); + EXPECT_EQ(7500U, vtolMcProtectionRthTransitionStartDistanceCm(800, 6200, 7500)); + EXPECT_EQ(7500U, vtolMcProtectionRthTransitionStartDistanceCm(0, 6200, 7500)); + EXPECT_EQ(1U, vtolMcProtectionRthTransitionStartDistanceCm(1, 1, 0)); + EXPECT_EQ(0U, vtolMcProtectionRthTransitionStartDistanceCm(2000, 0, 0)); + EXPECT_EQ(UINT32_MAX, vtolMcProtectionRthTransitionStartDistanceCm(UINT32_MAX, UINT32_MAX, 0)); +} + +TEST(VtolMcProtectionLogicTest, RthClosingSpeedProjectsVelocityTowardHome) +{ + EXPECT_EQ(2000U, vtolMcProtectionRthClosingSpeedCmS(true, 10000, 0, 2000, 500)); + EXPECT_EQ(2000U, vtolMcProtectionRthClosingSpeedCmS(true, 0, -10000, 500, -2000)); + EXPECT_EQ(0U, vtolMcProtectionRthClosingSpeedCmS(true, 10000, 0, -2000, 0)); + EXPECT_EQ(0U, vtolMcProtectionRthClosingSpeedCmS(true, 10000, 0, 0, 2000)); + EXPECT_EQ(1400U, vtolMcProtectionRthClosingSpeedCmS(true, -3000, -4000, -1000, -1000)); +} + +TEST(VtolMcProtectionLogicTest, RthClosingSpeedFallsBackForUnusableEstimates) +{ + EXPECT_EQ(0U, vtolMcProtectionRthClosingSpeedCmS(false, 10000, 0, 2000, 0)); + EXPECT_EQ(0U, vtolMcProtectionRthClosingSpeedCmS(true, 0, 0, 2000, 0)); + EXPECT_EQ(0U, vtolMcProtectionRthClosingSpeedCmS(true, NAN, 0, 2000, 0)); + EXPECT_EQ(0U, vtolMcProtectionRthClosingSpeedCmS(true, 10000, 0, INFINITY, 0)); + EXPECT_EQ(0U, vtolMcProtectionRthClosingSpeedCmS(true, 10000, 0, NAN, 0)); + EXPECT_EQ(0U, vtolMcProtectionRthClosingSpeedCmS(true, INFINITY, 0, 2000, 0)); +} + +TEST(VtolMcProtectionLogicTest, RthPredictionRejectsShortSpeedSpikeAndRestartsConfirmation) +{ + vtolMcProtectionSettleState_t state = {}; + const auto update = [&](uint32_t speed, timeMs_t now) { + const uint32_t startDistance = vtolMcProtectionRthTransitionStartDistanceCm(speed, 6200, 7500); + return vtolMcProtectionUpdateSettleState(&state, 10000U <= startDistance, + VTOL_MC_RTH_TRANSITION_TRIGGER_CONFIRM_MS, now); + }; + EXPECT_FALSE(update(2000, 100)); + EXPECT_FALSE(update(2000, 300)); + EXPECT_FALSE(update(800, 350)); + EXPECT_FALSE(update(2000, 400)); + EXPECT_FALSE(update(2000, 699)); + EXPECT_TRUE(update(2000, 700)); + // The production FSM clears this state on mode changes / a blocked route. + state = {}; + EXPECT_FALSE(update(2000, 10000)); + EXPECT_TRUE(update(2000, 10300)); +} + +TEST(VtolMcProtectionLogicTest, RthConfirmationHandlesMillisecondRollover) +{ + vtolMcProtectionSettleState_t state = {}; + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, 300, UINT32_MAX - 100)); + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, 300, 198)); + EXPECT_TRUE(vtolMcProtectionUpdateSettleState(&state, true, 300, 199)); +} + TEST(VtolMcProtectionLogicTest, PositionCaptureDoesNotOverrideNavigationOrPilotTarget) { EXPECT_TRUE(vtolMcProtectionPositionCaptureAllowed(true, true, false, false));