diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 685e17a4e70..6a05894a46d 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -22,6 +22,34 @@ 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 `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. + +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) + - 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. + ## Servo `Mixer Transition` is the input source for transition input; use this to tilt motor to gain airspeed. @@ -72,6 +100,214 @@ 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 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). + +### 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 `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. + +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) + +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`), +- 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. +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: + +- `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) +- `mixer_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: + +- `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` + - `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 +- 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). + +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). + +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 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) + +- 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). + +### 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]` = 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]` = 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/docs/Navigation.md b/docs/Navigation.md index 4d7b9740d91..9fb02f8a7a0 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -102,6 +102,45 @@ 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. +- 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 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. + +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. +- 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. + +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`). `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 d49067357bc..ece1eb3655e 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 | | --- @@ -3199,6 +3200,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. @@ -3209,6 +3220,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. For consistent manual transition semantics, enable this in both mixer profiles. + +| 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. @@ -4626,6 +4677,77 @@ 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_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. + +| 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. @@ -6974,6 +7096,56 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, --- +### 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. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### 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. + +| 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 `mixer_vtol_transition_dynamic_mixer` is ON. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### 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. diff --git a/docs/VTOL.md b/docs/VTOL.md index 8341c81086d..f4f93eefb25 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,306 @@ 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. +- `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 + +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. +- 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 `mixer_vtol_manualswitch_autotransition_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. + +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) +- 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. + +### 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 +- 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: +- `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 + +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 = 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). + +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, +- transition completion still follows airspeed thresholds when pitot is healthy, +- if pitot is unavailable/unhealthy, completion fallback still uses 5s. + +### 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 + +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. + +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. + +## 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 mixer_vtol_manualswitch_autotransition_controller = ON|OFF` + - Enables edge-triggered manual transition controller. + +- `set mixer_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 mixer_vtol_transition_airspeed_timeout_ms = ` + - Transition timeout/abort window. + +- `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>` + - 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 (absolute per-waypoint desired state). + +- `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. + +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 @@ -297,3 +597,43 @@ If you set `mixer_automated_switch` to `OFF` for all mixer profiles (the default - 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 (`mixer_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 `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: + +`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/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/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/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/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/fc/settings.yaml b/src/main/fc/settings.yaml index c53ef66852f..ef9ae02a2fb 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 @@ -171,6 +171,15 @@ 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: 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 @@ -1277,6 +1286,34 @@ groups: 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 + 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." + 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." + default_value: 0 + field: mixer_config.vtolTransitionAirspeedTimeoutMs + 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." + default_value: 0 + field: mixer_config.vtolTransitionScaleRampTimeMs + min: 0 + max: 60000 - name: tailsitter_orientation_offset description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode" default_value: OFF @@ -2623,6 +2660,38 @@ 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. 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 + - 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_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. 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 - 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 @@ -3957,6 +4026,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.c b/src/main/flight/mixer.c index a80992b772d..77038141281 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,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; + 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); } } @@ -737,4 +749,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 a39fbfeedd9..a7f01253cb0 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" @@ -17,6 +18,8 @@ #include "flight/failsafe.h" #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" @@ -29,6 +32,7 @@ #include "navigation/navigation.h" #include "common/log.h" +#include "build/debug.h" mixerConfig_t currentMixerConfig; int currentMixerProfileIndex; @@ -36,8 +40,11 @@ bool isMixerTransitionMixing; bool isMixerTransitionMixing_requested; 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, 1); +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 3); void pgResetFn_mixerProfiles(mixerProfile_t *instance) { @@ -53,6 +60,11 @@ 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, + .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, @@ -108,8 +120,212 @@ void mixerConfigInit(void) void setMixerProfileAT(void) { - mixerProfileAT.transitionStartTime = millis(); - mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; + const timeMs_t now = millis(); + + mixerProfileAT.transitionStartTime = now; + mixerProfileAT.aborted = false; + mixerProfileAT.abortedByAirspeedTimeout = 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; + 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 || + required_action == MIXERAT_REQUEST_MANUAL_TO_FW; +} + +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 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); +} + +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 + 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 (systemConfig()->vtolTransitionToFwMinAirspeed > 0) { + return systemConfig()->vtolTransitionToFwMinAirspeed; + } + return currentMixerConfig.switchTransitionAirspeed; + } + + if (direction == MIXERAT_DIRECTION_TO_MC) { + return systemConfig()->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(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) { + 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, 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); +} + +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(); +} + +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; + } + + 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; } bool platformTypeConfigured(flyingPlatformType_e platformType) @@ -120,6 +336,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 +360,28 @@ 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(); + + 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 false; } bool mixerATUpdateState(mixerProfileATRequest_e required_action) @@ -152,34 +391,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(false); 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 (millis() > mixerProfileAT.transitionTransEndTime){ + if (required_action != MIXERAT_REQUEST_NONE && required_action != mixerProfileAT.request) { + abortTransition(false); + return true; + } + + if (mixerATReadyForHotSwitch(mixerProfileAT.request)) { isMixerTransitionMixing_requested = false; - outputProfileHotSwitch(nextMixerProfileIndex); + mixerProfileAT.progress = 1.0f; + updateTransitionScales(); + 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; - //transition is done + } else if (mixerProfileAT.usedAirspeed && + currentMixerConfig.vtolTransitionAirspeedTimeoutMs > 0 && + (millis() - mixerProfileAT.transitionStartTime) >= currentMixerConfig.vtolTransitionAirspeedTimeoutMs) { + abortTransition(true); + return true; } + + updateTransitionScales(); return false; break; default: @@ -202,17 +464,192 @@ bool checkMixerProfileHotSwitchAvalibility(void) void outputProfileUpdateTask(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - if(cliMode) return; - 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 (cliMode) { + return; + } + + 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 || 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 = manualControllerEnabled && transitionModeActive; + if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse && !suppressDirectProfileSwitch) { 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)); + + // Recompute after a potential direct profile hot-switch because this flag is per-mixer-profile. + manualControllerEnabled = (currentMixerConfig.manualVtolTransitionController && !missionActive) || manualTransitionSessionLatched; + + if (!manualControllerEnabled) { + // 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 { + if (!transitionModeActive) { + manualTransitionSessionLatched = false; + 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(false); + 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)); + + 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) | + (manualTransitionSessionLatched ? 1U << 20 : 0U); + + // 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 + DEBUG_SET(DEBUG_VTOL_TRANSITION, 0, mixerProfileAT.phase); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, mixerProfileAT.request); + 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); + + if (!isMixerTransitionMixing) { + resetTransitionScales(); + } +} + +bool mixerATIsActive(void) +{ + return mixerProfileAT.phase != MIXERAT_PHASE_IDLE; +} + +bool mixerATWasAborted(void) +{ + return mixerProfileAT.aborted; +} + +bool mixerATWasAbortedByAirspeedTimeout(void) +{ + return mixerProfileAT.abortedByAirspeedTimeout; +} + +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); +} + +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 (or session latched), treat RC as trigger only. + if (currentMixerConfig.manualVtolTransitionController || manualTransitionSessionLatched) { + return false; + } + + return IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); } // switch mixerprofile without reboot diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 715732d0685..59fe4384809 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,6 +18,11 @@ typedef struct mixerConfig_s { bool controlProfileLinking; bool automated_switch; int16_t switchTransitionTimer; + uint16_t switchTransitionAirspeed; + bool vtolTransitionDynamicMixer; + bool manualVtolTransitionController; + uint16_t vtolTransitionAirspeedTimeoutMs; + uint16_t vtolTransitionScaleRampTimeMs; bool tailsitterOrientationOffset; int16_t transition_PID_mmix_multiplier_roll; int16_t transition_PID_mmix_multiplier_pitch; @@ -34,27 +39,57 @@ 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_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, MIXERAT_PHASE_TRANSITION_INITIALIZE, MIXERAT_PHASE_TRANSITIONING, - MIXERAT_PHASE_DONE, } mixerProfileATState_e; typedef struct mixerProfileAT_s { mixerProfileATState_e phase; - bool transitionInputMixing; + mixerProfileATDirection_e direction; + mixerProfileATRequest_e request; + bool aborted; + bool abortedByAirspeedTimeout; + bool hotSwitchDone; + bool usedAirspeed; + bool transitionStartAirspeedCaptured; + float progress; + float transitionStartAirspeedCmS; + float blendToFw; + float pusherScale; + float liftScale; + 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); bool mixerATUpdateState(mixerProfileATRequest_e required_action); +bool mixerATIsActive(void); +bool mixerATWasAborted(void); +bool mixerATWasAbortedByAirspeedTimeout(void); +float mixerATGetPusherScale(void); +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; @@ -81,4 +116,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/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 8f60155b68c..10a9618a82a 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,13 @@ #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 +#define NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS 6000 +#define NAV_MIXERAT_RETRY_MAX_TOTAL_MS 45000 + /*----------------------------------------------------------- * Compatibility for home position *-----------------------------------------------------------*/ @@ -119,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, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -149,6 +157,12 @@ 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, + .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 @@ -270,6 +284,46 @@ uint16_t navEPV; int16_t navAccNEU[3]; //End of blackbox states +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; + bool retryHadTrustedAirspeedSample; + timeMs_t retryStartTimeMs; + timeMs_t retryStepStartTimeMs; + timeMs_t retryHeadingReachedTimeMs; + 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_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, + 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 +342,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 +350,14 @@ 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); +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 navMixerATRetryScanResult_e updateMissionTransitionRetryScan(void); void missionPlannerSetWaypoint(void); void initializeRTHSanityChecker(void); @@ -1044,8 +1107,12 @@ 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, } }, [NAV_STATE_MIXERAT_ABORT] = { @@ -1258,7 +1325,25 @@ 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; + 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 (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; } flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) @@ -1282,6 +1367,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState { UNUSED(previousState); + navMixerATPendingState = NAV_STATE_IDLE; + clearMissionVTOLTransitionState(); resetAltitudeController(false); resetHeadingController(); resetPositionController(); @@ -1965,6 +2052,345 @@ 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; +} + +#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) || !pitotValidForAirspeed() || 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) +{ + 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; + 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 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; + } +} + +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.retryHadTrustedAirspeedSample = false; + navMixerATMissionTransition.retryStartTimeMs = 0; + navMixerATMissionTransition.retryStepStartTimeMs = 0; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 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.retryHadTrustedAirspeedSample = false; + navMixerATMissionTransition.retryStartTimeMs = millis(); + navMixerATMissionTransition.retryStepStartTimeMs = navMixerATMissionTransition.retryStartTimeMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; +} + +static navMixerATRetryScanResult_e updateMissionTransitionRetryScan(void) +{ + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_IDLE) { + 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)); + 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.retryHeadingReachedTimeMs) < 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)) { + 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 = 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; + } + + 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) +{ + 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() || + mixerATIsActive()) { + 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.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)) { + 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 +2399,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 +2722,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 +2731,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; } @@ -2302,6 +2744,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) { @@ -2311,32 +2760,88 @@ 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; } + + 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; + } + if (mixerATUpdateState(required_action)){ // MixerAT is done, switch to next state + 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); - switch (navMixerATPendingState) - { - case NAV_STATE_RTH_HEAD_HOME: + clearMissionVTOLTransitionState(); + if (nextEvent != NAV_FSM_EVENT_SWITCH_TO_IDLE) { 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; } + return nextEvent; } - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + updateMissionTransitionGuidance(); return NAV_FSM_EVENT_NONE; } @@ -2345,6 +2850,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigatio { UNUSED(previousState); mixerATUpdateState(MIXERAT_REQUEST_ABORT); + clearMissionVTOLTransitionState(); return NAV_FSM_EVENT_SUCCESS; } @@ -5062,6 +5568,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..03fc33f3d81 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -328,6 +328,29 @@ 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 { + 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, @@ -413,6 +436,12 @@ 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 + 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 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,