diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 685e17a4e70..a55f8d10962 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -12,15 +12,60 @@ For VTOL setup. one mixer_profile is used for multi-rotor(MR) and the other is u By default, switching between profiles requires reboot to take affect. However, using the RC mode: `MIXER PROFILE 2` will allow in flight switching for things like VTOL operation . And will re-initialize pid and navigation controllers for current MC or FW flying mode. +For consistency, this document uses the long-standing VTOL order: +- Profile 1 = fixed-wing (FW) +- Profile 2 = multicopter (MC) + +The firmware can work with the profiles swapped, but the examples below keep this order so the switch logic is easier to follow. + Please note that this is an emerging / experimental capability that will require some effort by the pilot to implement. ## Mixer Transition input -Typically, 'transition input' will be useful in MR mode to gain airspeed. -The associated motor or servo will then move accordingly when transition mode is activated. -Transition input is disabled when navigation mode is activate +`MIXER TRANSITION` is mainly used while the model is still in multicopter mode, so a forward motor or tilt servo can help the aircraft build forward speed before the switch to fixed-wing. + +This feature is recommended for VTOL setups. It is normally blocked while navigation modes are active. +Mapping a motor to a servo output, or using servo logic conditions for this feature, is **not** recommended. + +If `mixer_vtol_manualswitch_autotransition_controller = ON`, `MIXER TRANSITION` works like a start switch for one transition: + +- Each time you move `MIXER TRANSITION` from OFF to ON, iNAV starts one transition. +- The same switch can be used in both directions: + - MC -> transition -> FW + - FW -> transition -> MC +- After it starts, the transition keeps running until the speed target or timer target is reached. +- Leaving the switch ON does not keep restarting the transition. +- To start another transition, turn the switch OFF and then ON again. +- If you turn the switch OFF before the profile change happens, that transition request is cancelled. +- Optional extra protection: `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can automatically start FW->MC if airspeed gets too low. After that switch, iNAV stays in MC until you deliberately command another manual profile change. + +This behavior is controlled by `mixer_vtol_manualswitch_autotransition_controller`. +Turn it ON in both mixer profiles if you want the same switch behavior in both directions. +If it is OFF, manual transition keeps the older behavior. + +In Active Modes: + +- `MIXER TRANSITION` shows that the internal transition logic is actually running. +- `MIXER PROFILE 2` shows that mixer profile 2 is currently active. + +There are two separate manual paths: + +- `MIXER PROFILE 2` is still a direct manual profile switch when `MIXER TRANSITION` is OFF. +- `MIXER TRANSITION` starts the smooth automatic transition sequence when `mixer_vtol_manualswitch_autotransition_controller = ON`. +- If both are ON together while the automatic transition controller is enabled, the controller temporarily owns the profile switching. When `MIXER TRANSITION` turns OFF again, direct `MIXER PROFILE 2` switching becomes active again. +- If low-speed protection switches the model from FW to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. -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 +3-position switch example: + +- This example assumes the usual VTOL order used in this document: + - Profile 1 = FW + - Profile 2 = MC +- One supported mapping is: + - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) + - Pos2 = Transition request (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) + - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) +- Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. +- If you intentionally swap the profile order, keep the same idea and swap the FW and MC end positions. ## Servo @@ -56,7 +101,11 @@ It is recommend that the pilot uses a RC mode switch to activate modes or switch Profile files Switching is not available until the runtime sensor calibration is done. Switching is NOT available when navigation mode is activate. `mixer_profile` 1 will be used as default, `mixer_profile` 2 will be used when the `MIXER PROFILE 2` mode box is activated. -Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. Here is sample of using these RC modes: +Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. + +Another supported mapping is where one switch position turns ON both `MIXER PROFILE 2` and `MIXER TRANSITION`. +With `mixer_vtol_manualswitch_autotransition_controller = OFF`, `MIXER TRANSITION` is used as a live transition input. +With `mixer_vtol_manualswitch_autotransition_controller = ON`, that same overlap position is used as a controller-owned transition position. ![Alt text](Screenshots/mixer_profile.png) @@ -66,11 +115,233 @@ Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input It is also possible to set it as 4 state switch by adding FW(profile1) with transition on. +If `mixer_vtol_manualswitch_autotransition_controller = ON` and this overlap position is active: +- the smooth transition controller runs while `MIXER TRANSITION` stays ON +- direct `MIXER PROFILE 2` switching is deferred during that time +- when `MIXER TRANSITION` turns OFF, `MIXER PROFILE 2` again decides which stable mixer profile should be active + +This overlap style is supported too. + ## Automated Transition -This feature is mainly for RTH in a failsafe event. When set properly, model will use the FW mode to fly home efficiently, And land in the MC mode for easier landing. -`ON` for a mixer_profile\`s `mixer_automated_switch` means to schedule a Automated Transition when RTH head home(applies for MC mixer_profile) or RTH Land(applies for FW mixer_profile) is requested by navigation controller. -Set `mixer_automated_switch` to `ON` in mixer_profile for MC mode. Set `mixer_switch_trans_timer` in mixer_profile for MC mode for the time required to gain airspeed for your model before entering to FW mode. -When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Model will not perform automated transition at all. +This feature is mainly used for RTH and failsafe. +When set up correctly, the aircraft can use fixed-wing for the efficient flight home and then return to multicopter for easier landing. + +If `mixer_automated_switch = ON` in a mixer profile, iNAV is allowed to run an automated transition when the navigation logic asks for it. + +- Use `mixer_automated_switch = ON` in the MC mixer profile if you want automated MC->FW transition during the head-home part of RTH. +- Use `mixer_automated_switch = ON` in the FW mixer profile if you want automated FW->MC transition for the landing part. +- Set `mixer_switch_trans_timer` in each profile to a sensible backup time for that direction. + +If `mixer_automated_switch = OFF` in all mixer profiles, automated VTOL transition is disabled. + +### Unified VTOL transition controller + +Manual `MIXER TRANSITION` and mission-requested VTOL transition both use the same internal transition controller. +That means the same airspeed checks, timer fallback, and smooth power changes are reused in both cases. + +Direct manual `MIXER PROFILE 2` switching is still a separate path when you want an immediate profile change. + +### Airspeed-first completion + +When valid pitot airspeed is available, iNAV uses airspeed to decide when the transition is complete: + +- `vtol_transition_to_fw_min_airspeed_cm_s` for MC->FW +- `vtol_transition_to_mc_max_airspeed_cm_s` for FW->MC + +If pitot is not available, not healthy, or the threshold is set to `0`, iNAV uses `mixer_switch_trans_timer` instead. +Ground speed is not used for transition completion/progress. + +The three timer settings do different jobs: + +- `mixer_switch_trans_timer` is the original VTOL transition timer. It is still the backup completion timer when trusted pitot airspeed is not being used. +- `mixer_vtol_transition_airspeed_timeout_ms` is only a maximum wait time for the required airspeed while pitot is still usable. It does not complete the transition by itself; it aborts that airspeed-controlled attempt. +- If pitot becomes unavailable during transition, iNAV stops using the airspeed timeout and falls back to `mixer_switch_trans_timer`. +- For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). + +### Smooth power changes during transition (optional) + +When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: + +- forward motor power, +- lift motor power, +- multicopter stabilisation strength, +- fixed-wing control strength. + +Default is OFF to preserve existing behavior. +With it ON, `vtol_transition_fw_authority_start_percent = 100` keeps the old fixed-wing control behavior. Lower values bring fixed-wing control in more gently. + +How `mixer_vtol_transition_scale_ramp_time_ms` works: + +- MC->FW pusher: + - `> 0`: forward motor power ramps from `0 -> 100%` over this time, even when pitot is working normally. + - `= 0` (default): forward motor power goes to `100%` immediately. +- This timer does not decide when the transition completes. +- Lift motor power, MC stabilisation, and FW control: + - with valid pitot airspeed, they still follow transition progress based on airspeed. + - if pitot stops being usable and this setting is `> 0`, they use this same timer as a backup ramp. + - if pitot stops being usable and this setting is `0`, they fall back to the normal transition timer/progress behavior. +- FW->MC keeps the existing style of smooth handover. + +Example: + +- `mixer_switch_trans_timer = 50` (5s fallback completion timer) +- `mixer_vtol_transition_scale_ramp_time_ms = 1200` + +Result: +- in MC->FW, the forward motor reaches full power in about `1.2s`, +- when pitot is working, lift power and control handover still follow airspeed, +- if pitot stops being usable, the same handover reaches its target in about `1.2s`, +- transition completion still uses airspeed when pitot is working, +- backup completion time is still `5s` if pitot is not usable. + +### Mission-authorized VTOL transition (waypoint User Action) + +INAV can also change between MC and FW from the mission itself. +This is configured with: + +- `nav_vtol_mission_transition_user_action` (`OFF`, `USER1`, `USER2`, `USER3`, `USER4`) +- `nav_vtol_mission_transition_min_altitude_cm` (optional, `0` disables minimum-altitude check) +- `vtol_transition_to_fw_min_airspeed_cm_s` (preferred MC->FW threshold) + +Setting scope: + +- Per-mixer-profile settings: + - `mixer_automated_switch` + - `mixer_switch_trans_timer` + - `mixer_vtol_transition_dynamic_mixer` + - `mixer_vtol_manualswitch_autotransition_controller` + - `mixer_vtol_transition_airspeed_timeout_ms` + - `mixer_vtol_transition_scale_ramp_time_ms` +- Global settings: + - `vtol_transition_to_fw_min_airspeed_cm_s` + - `vtol_transition_to_mc_max_airspeed_cm_s` + - `vtol_fw_to_mc_auto_switch_airspeed_cm_s` + - `vtol_transition_lift_end_percent` + - `vtol_transition_mc_authority_end_percent` + - `vtol_transition_fw_authority_start_percent` + - `nav_vtol_mission_transition_user_action` + - `nav_vtol_mission_transition_min_altitude_cm` + +On each navigable mission waypoint (`WAYPOINT`, `POSHOLD_TIME`, `LAND`), the chosen USER flag tells iNAV which flight mode should be active there: + +- selected USER flag = `0` -> target MC / MULTIROTOR profile +- selected USER flag = `1` -> target FW / AIRPLANE profile +- This is set per waypoint. It is **not** a toggle command. +- If the aircraft is already in the requested mode, iNAV does nothing and continues. + +The mission pauses while transition is in progress and resumes after completion. + +For MC->FW mission transitions, navigation uses a built-in straight acceleration run to build speed before the switch to fixed-wing. +Mission transition uses the same transition logic as manual transition: airspeed first, timer as backup. + +Manual RC switching (`MIXER PROFILE 2`, `MIXER TRANSITION`) is still blocked during normal active navigation. +Mission VTOL transition still relies on the normal two-profile setup, so you must configure both mixer profiles and a valid `MIXER PROFILE 2` mode condition. + +### Example test presets (VTOL ~1.0m wingspan, ~1720g AUW) + +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 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: +- Keeps behavior close to the older transition setup. +- Good as a known-safe starting point before enabling the smoother power changes. + +#### Test 2 - Airspeed-first + smooth power changes (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 airspeed first, with timer fallback only if pitot is not usable. +- Adds smoother forward-motor, lift-motor, and control handover to reduce abrupt transitions. + +#### Test 3 - Mission-authorized transition (mission integration) + +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` + +Behavior: +- Uses USER1 as the per-waypoint target selector (`clear = MC`, `set = FW`). +- Pauses mission progression during transition and resumes only after transition completion. + +### Validation Matrix (PR / SITL / HITL) + +- 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 profile change (safe abort). +- Mission transition with selected USER bit = `1` (TO_FW). +- Mission transition with selected USER bit = `0` (TO_MC). +- Failsafe/disarm during active transition (abort and no blind mission resume). + +### 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: profile change done + - bit7: transition aborted + - bit8: manual VTOL auto-transition controller enabled in current mixer config + - bit9: dynamic transition mixer enabled in current mixer config + - 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 stabilisation scale x1000 (`0..1000`) +- `debug[7]` = current mixer profile pitch transition PID multiplier (`transition_PID_mmix_multiplier_pitch`) ## TailSitter (planned for INAV 7.1) 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..e3598cf5fc0 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -102,6 +102,44 @@ 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). +- During MC->FW mission transition, iNAV uses a built-in straight run-up target to help the model build speed before switching to fixed-wing. +- VTOL transition completion logic is shared with manual MIXER TRANSITION and uses mixer transition settings: + - preferred MC->FW threshold: `vtol_transition_to_fw_min_airspeed_cm_s` + - FW->MC threshold: `vtol_transition_to_mc_max_airspeed_cm_s` + +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 is `0`), timer fallback (`mixer_switch_trans_timer`) is used. +- Ground speed is not used for transition progress/completion. +- FW -> MC: mission pauses during automated transition, then resumes after switching back to MC profile. +- Strict altitude hold is not enforced during MC -> FW transition; natural climb is allowed. + +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..76ef09a71e5 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 | | --- @@ -3201,7 +3202,7 @@ If enabled, control_profile_index will follow mixer_profile index. Set to OFF(de ### mixer_switch_trans_timer -If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. +Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing. | Default | Min | Max | | --- | --- | --- | @@ -3209,6 +3210,46 @@ If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_ --- +### mixer_vtol_manualswitch_autotransition_controller + +Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switch moves from OFF to ON, when not in waypoint mission. Turn this ON in both mixer profiles if you want the same behavior in both directions. OFF keeps the older manual switch behavior. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mixer_vtol_transition_airspeed_timeout_ms + +Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + +### mixer_vtol_transition_dynamic_mixer + +Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mixer_vtol_transition_scale_ramp_time_ms + +When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable. + +| 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 +4667,67 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT --- +### nav_vtol_mission_transition_min_altitude_cm + +Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the altitude check. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 50000 | + +--- + +### nav_vtol_mission_transition_user_action + +Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles and a working `MIXER PROFILE 2` mode setup. + +| Allowed Values | | +| --- | --- | +| OFF | Default | +| USER1 | | +| USER2 | | +| USER3 | | +| USER4 | | + +--- + +### nav_vtol_transition_fail_action_fw_to_mc + +What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. + +| Allowed Values | | +| --- | --- | +| IDLE | | +| LOITER | Default | +| RTH | | +| EMERGENCY_LANDING | | +| FORCE_SWITCH | | + +--- + +### nav_vtol_transition_fail_action_mc_to_fw + +What iNAV should do if MC->FW transition still fails after the final attempt. + +| Allowed Values | | +| --- | --- | +| IDLE | Default | +| POSH | | +| RTH | | +| EMERGENCY_LANDING | | + +--- + +### nav_vtol_transition_retry_on_airspeed_timeout + +If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. + +| 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 +7076,66 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, --- +### vtol_fw_to_mc_auto_switch_airspeed_cm_s + +Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20000 | + +--- + +### vtol_transition_fw_authority_start_percent + +How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring in fixed-wing control more gently. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_lift_end_percent + +How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_mc_authority_end_percent + +How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_to_fw_min_airspeed_cm_s + +Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20000 | + +--- + +### vtol_transition_to_mc_max_airspeed_cm_s + +When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. + +| 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..396deb0c5e3 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -23,15 +23,21 @@ We highly value your feedback as it plays a crucial role in the development and # VTOL Configuration Steps ### The VTOL functionality is achieved by switching/transitioning between two configurations stored in the FC. VTOL specific configurations are Mixer Profiles with associated control profiles. One profile set is for fixed-wing(FW) mode, One is for multi-copter(MC) mode. Configuration/Settings other than Mixer/control profiles are shared among two modes + +This guide uses the long-standing VTOL setup order: +- Profile 1 = fixed-wing (FW) +- Profile 2 = multicopter (MC) + +The firmware can work with the profiles swapped, but keeping one order in the guide makes the setup steps easier to follow. ![Alt text](Screenshots/mixerprofile_flow.png) 0. **Find a DIFF ALL file for your model and start from there if possible** - Be aware that `MIXER PROFILE 2` RC mode setting introduced by diff file can get your stuck in a mixer_profile. remove or change channel to proceed 1. **Setup Profile 1:** - - Configure it as a normal fixed-wing/multi-copter. + - Configure it as your normal fixed-wing setup. 2. **Setup Profile 2:** - - Configure it as a normal multi-copter/fixed-wing. + - Configure it as your normal multicopter setup. 3. **Mode Tab Settings:** - Set up switching in the mode tab. @@ -125,8 +131,8 @@ save save ``` -2. **Configure the fixed-wing/Multi-Copter:** - - Configure your fixed-wing/Multi-Copter as you normally would, or you can copy and paste default settings to expedite the process. +2. **Configure the fixed-wing:** + - Configure your fixed-wing as you normally would, or you can copy and paste default settings to speed things up. - Dshot esc protocol availability might be limited depends on outputs and fc board you are using. change the motor wiring or use oneshot/multishot esc protocol and calibrate throttle range. - You can use throttle = -1 as a placeholder for the motor you wish to stop if the motor isn't the last motor - Consider conducting a test flight to ensure that everything operates as expected. And tune the settings, trim the servos. @@ -147,8 +153,8 @@ You must also assign the tilting servos values using the MAX values. If you don save ``` -2. **Configure the Multicopter/tricopter:** - - Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and control_profile 2. +2. **Configure the multicopter/tricopter:** + - Set up your multicopter/tricopter as usual, this time for mixer_profile 2 and control_profile 2. - Utilize the 'MAX' input in the servo mixer to tilt the motors without altering the servo midpoint. - At this stage, focus on configuring profile-specific settings. You can streamline this process by copying and pasting the default PID settings. - you can set -1 in motor mixer throttle as a place holder: this will disable that motor but will load following the motor rules @@ -174,6 +180,12 @@ You must also assign the tilting servos values using the MAX values. If you don | :-- | :-- | :-- | | Profile1(FW) with transition off | Profile2(MC) with transition on | Profile2(MC) with transition off | +- This is one supported mapping, where one switch position turns ON both `MIXER PROFILE 2` and `MIXER TRANSITION`. +- With `mixer_vtol_manualswitch_autotransition_controller = OFF`, `MIXER TRANSITION` is used as a live transition input. +- With `mixer_vtol_manualswitch_autotransition_controller = ON`, that same overlap position is used as a controller-owned transition position. +- While both are ON, the smooth transition controller runs and direct `MIXER PROFILE 2` switching is deferred. +- When `MIXER TRANSITION` turns OFF again, `MIXER PROFILE 2` once more decides which stable mixer profile should be active. + - Profile file switching becomes available after completing the runtime sensor calibration (15-30s after booting). And It is **not available** when a navigation mode or position hold is active. - By default, `mixer_profile 1` is used. `mixer_profile 2` is used when the `MIXER PROFILE 2` mode is activate. Once configured successfully, you will notice that the profiles and model preview changes accordingly when you refresh the relevant INAV Configurator tabs. @@ -239,7 +251,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 +295,312 @@ 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 changes and avoids separate transition implementations. + +### Behavior summary + +- iNAV always tracks transition progress internally. +- If valid pitot airspeed is available, airspeed is the main way iNAV decides when transition is complete. +- If pitot is not available, iNAV falls back to a timer. +- Ground speed is not used for transition completion. +- Mission VTOL transition uses the same controller and does not directly drive the motors by itself. +- During normal waypoint navigation, manual `MIXER PROFILE` and `MIXER TRANSITION` switching is still blocked. +- `MIXER PROFILE 2` is still a direct manual profile switch when you are flying manually. +- Smooth automatic transition is started by `MIXER TRANSITION` when the manual auto-controller is ON, or by a mission transition request. + +### Manual transition semantics + +This does not remove the older manual behavior. The older behavior is still available if you want it. + +With `mixer_vtol_manualswitch_autotransition_controller = ON`: +- Turn this ON in both mixer profiles if you want the same behavior in both directions. +- Each time `MIXER TRANSITION` moves from OFF to ON, iNAV starts one transition. +- After it starts, the transition keeps running until the speed target or timer target is reached. +- Leaving the switch ON does not keep restarting the transition. +- To start another transition, turn the switch OFF and then ON again. +- If you turn the switch OFF before the profile change happens, that transition request is cancelled. +- Optional extra protection: set `vtol_fw_to_mc_auto_switch_airspeed_cm_s > 0` if you want FW->MC to start automatically when pitot airspeed becomes too low. After that switch, iNAV stays in MC until you deliberately command another manual profile change. + +With `mixer_vtol_manualswitch_autotransition_controller = OFF`: +- the older manual behavior is preserved. + +Typical 3-position switch workflow: +- Position 1: FW +- Position 2: Transition request +- Position 3: MC + +Operational example: +- fly in MC (pos3) -> move to Transition (pos2) to start automatic MC->FW transition -> after completion move to FW (pos1) +- reverse the order for FW->MC + +Important RC mapping constraint: +- One supported mapping is: + - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) + - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) + - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) +- Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. +- Another supported mapping is the overlap version: while both `MIXER PROFILE 2` and `MIXER TRANSITION` are ON, the transition controller owns the switching until `MIXER TRANSITION` turns OFF again. + +### Mission-authorized transition semantics + +Mission transition is configured with `nav_vtol_mission_transition_user_action`. + +- `OFF`: feature disabled. +- `USER1`..`USER4`: the selected USER flag becomes the flight-mode selector on navigable waypoints. +- selected flag `0` -> target MC profile +- selected flag `1` -> target FW profile +- when this feature is ON, every navigable waypoint should intentionally have that USER flag either clear or set +- Mission progression pauses during transition and resumes only after completion. +- If the aircraft is already in the requested mode, iNAV does nothing and continues. + +For MC -> FW mission transition: +- guidance uses a straight acceleration run, +- normal waypoint advancement is paused during transition. + +### Airspeed-first completion logic + +MC -> FW: +- `vtol_transition_to_fw_min_airspeed_cm_s` is the target airspeed. +- If pitot stops being usable, or if this is `0`, MC->FW uses `mixer_switch_trans_timer` instead. + +FW -> MC: +- `vtol_transition_to_mc_max_airspeed_cm_s` is the airspeed that must be reached or lower. +- If pitot stops being usable, or if this is `0`, FW->MC uses `mixer_switch_trans_timer` instead. + +Timeout: +- `mixer_switch_trans_timer` is the original VTOL transition timer. It is still the backup completion timer when trusted pitot airspeed is not being used. +- `mixer_vtol_transition_airspeed_timeout_ms` is only a maximum wait time for the required airspeed while pitot is still usable. It does not complete the transition by itself; it aborts that airspeed-controlled attempt. +- If pitot stops being usable, iNAV stops using the airspeed timeout and falls back to `mixer_switch_trans_timer`. +- For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). + +### Smooth power changes during transition + +When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: +- forward motor power, +- lift motor power, +- multicopter stabilisation strength, +- fixed-wing control strength. + +When `mixer_vtol_transition_dynamic_mixer = OFF`, the older static behavior is preserved. + +`mixer_vtol_transition_scale_ramp_time_ms` always controls the MC->FW forward-motor ramp when this feature is ON. +It does not decide when the transition completes. + +How `mixer_vtol_transition_scale_ramp_time_ms` works: +- MC->FW pusher: + - `> 0`: forward motor power ramps from `0 -> 100%` over this time, even when pitot is working normally. + - `= 0` (default): forward motor power goes to `100%` immediately. +- Lift motor power, MC stabilisation, and FW control: + - with valid pitot airspeed, they still follow airspeed-based transition progress. + - if pitot stops being usable and this setting is `> 0`, they use this same timer as a backup ramp. + - if pitot stops being usable and this setting is `0`, they fall back to the normal transition timer/progress behavior. +- FW->MC keeps the existing style of smooth handover. + +Example: +- `mixer_switch_trans_timer = 50` (5s fallback completion timer) +- `mixer_vtol_transition_scale_ramp_time_ms = 1200` + +Result: +- in MC->FW, the forward motor reaches full power in about `1.2s`, +- when pitot is working, lift power and control handover still follow airspeed, +- if pitot stops being usable, the same handover reaches its target in about `1.2s`, +- transition completion still uses airspeed when pitot is working, +- backup completion time is still `5s` if pitot is not usable. + +### Example test presets (VTOL ~1.0m wingspan, ~1720g AUW) + +These are example starting points for initial testing. They are not universal values; tune after bench tests and short flight tests. + +#### Test 1 - Legacy-compatible baseline (manual transition check) + +Goal: +- Verify that the new controller does not change legacy behavior when smooth power changes are disabled. +- Good first test after flashing. + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = OFF` +- `set mixer_switch_trans_timer = 45` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 0` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 900` +- `set mixer_vtol_transition_airspeed_timeout_ms = 0` +- `set mixer_vtol_transition_scale_ramp_time_ms = 0` +- `set nav_vtol_mission_transition_user_action = OFF` + +What this does: +- Keeps transition mixing behavior close to legacy mode. +- Uses timer-driven completion when no trusted pitot threshold is configured. +- Uses conservative FW->MC completion threshold. +- Disables mission-authorized transition while validating manual behavior. + +#### Test 2 - Airspeed-first + smooth power changes (manual transition tuning) + +Goal: +- Enable the full new behavior: airspeed-first completion and smooth forward-motor and control handover. + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` +- `set mixer_switch_trans_timer = 50` +- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` +- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `set vtol_transition_lift_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. +- In MC->FW, the forward motor ramps to full power in `1.2s` while lift power and control handover still follow airspeed progress. +- The pusher ramp is quick enough (1.2 s) to reduce step torque while still allowing strong acceleration. +- Timeout abort protects against staying too long in airspeed-controlled transition without reaching threshold. + +#### Test 3 - Mission-authorized transition (end-to-end mission flow) + +Goal: +- Validate mission User Action integration and pause/resume behavior. + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` +- `set mixer_switch_trans_timer = 50` +- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` +- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `set vtol_transition_lift_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` + +What this does: +- Uses USER1 as the absolute per-waypoint target selector: + - USER1 bit clear -> target MC + - USER1 bit set -> target FW +- Pauses mission progression during transition and resumes after completion. +- Uses a straight MC->FW acceleration segment (no loiter) before the switch to fixed-wing. +- Adds a minimum altitude gate (12 m) before mission transition starts. + +### Detailed effect of the three percentage settings + +These three settings are active only when `mixer_vtol_transition_dynamic_mixer = ON`. + +1. `vtol_transition_lift_end_percent` +- Sets how much lift motor power remains at the end of transition. +- MC -> FW: lift power goes from `100%` at start to `lift_end_percent` at the end. +- FW -> MC: lift power goes from `lift_end_percent` at start to `100%` at the end. + +Example (`vtol_transition_lift_end_percent = 20`): +- MC -> FW at 50% progress: lift power is about `60%`. +- FW -> MC at 50% progress: lift power is about `60%`. + +2. `vtol_transition_mc_authority_end_percent` +- Sets how much multicopter stabilisation remains at the end of transition. +- MC -> FW: MC stabilisation goes from `100%` at start to `mc_authority_end_percent` at the end. +- FW -> MC: MC stabilisation goes from `mc_authority_end_percent` at start to `100%` at the end. + +Example (`vtol_transition_mc_authority_end_percent = 30`): +- MC -> FW at 50% progress: MC stabilisation is about `65%`. +- FW -> MC at 50% progress: MC stabilisation is about `65%`. + +3. `vtol_transition_fw_authority_start_percent` +- Sets how much fixed-wing control is already available at the start of transition. +- MC -> FW: fixed-wing control goes from `fw_authority_start_percent` at start to `100%` at the end. +- FW -> MC: fixed-wing control goes from `100%` at start to `fw_authority_start_percent` at the end. + +Example (`vtol_transition_fw_authority_start_percent = 25`): +- MC -> FW at 50% progress: fixed-wing control is about `62.5%`. +- FW -> MC at 50% progress: fixed-wing control is about `62.5%`. + +Backward-compatible note: +- `vtol_transition_fw_authority_start_percent = 100` keeps the older fixed-wing control behavior. +- Lower values bring fixed-wing control in and out more gently. + +## Setting Scope (Important) + +The new VTOL settings are split into two groups: + +### Per-mixer-profile settings + +In the examples in this guide, mixer profile 1 is FW and mixer profile 2 is MC. +These settings can differ between the two mixer profiles: + +- `mixer_automated_switch` +- `mixer_switch_trans_timer` +- `mixer_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_fw_to_mc_auto_switch_airspeed_cm_s` +- `vtol_transition_lift_end_percent` +- `vtol_transition_mc_authority_end_percent` +- `vtol_transition_fw_authority_start_percent` +- `nav_vtol_mission_transition_user_action` +- `nav_vtol_mission_transition_min_altitude_cm` + +## CLI Commands (English) + +Use these commands in CLI (`set ...`, then `save`): + +- `set mixer_vtol_manualswitch_autotransition_controller = ON|OFF` + - Makes `MIXER TRANSITION` start one automatic transition each time you turn it ON. + +- `set mixer_vtol_transition_dynamic_mixer = ON|OFF` + - Turns smooth transition power changes ON or OFF. + +- `set vtol_transition_to_fw_min_airspeed_cm_s = ` + - Preferred MC -> FW completion threshold (pitot airspeed). + +- `set mixer_switch_trans_timer = ` + - Backup transition time used when pitot airspeed is not available. + +- `set vtol_transition_to_mc_max_airspeed_cm_s = ` + - FW -> MC completion threshold (pitot airspeed). + +- `set vtol_fw_to_mc_auto_switch_airspeed_cm_s = ` + - Optional low-speed protection threshold for fixed-wing. After it switches to MC, iNAV stays in MC until you deliberately command another manual profile change (`0` disables). + +- `set mixer_vtol_transition_airspeed_timeout_ms = ` + - How long iNAV waits for required pitot airspeed before aborting. + +- `set mixer_vtol_transition_scale_ramp_time_ms = ` + - Ramp-up time for the forward motor, and backup ramp time for the other smooth transition power changes. + +- `set vtol_transition_lift_end_percent = <0..100>` + - How much lift motor power remains at the end of transition. + +- `set vtol_transition_mc_authority_end_percent = <0..100>` + - How much multicopter stabilisation remains at the end of transition. + +- `set vtol_transition_fw_authority_start_percent = <0..100>` + - How much fixed-wing control is already available at the start of transition. + +- `set nav_vtol_mission_transition_user_action = OFF|USER1|USER2|USER3|USER4` + - Selects which waypoint USER flag tells iNAV to use MC or FW at each waypoint. + +- `set nav_vtol_mission_transition_min_altitude_cm = ` + - Optional minimum altitude before mission transition may start (`0` disables). + +Mission profile-switch dependency: +- Mission VTOL transition uses the existing profile-change path, so two valid mixer profiles and a configured `MIXER PROFILE 2` mode activation condition are required. + # Notes and Experiences ## General @@ -297,3 +615,48 @@ 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` + +Smooth transition power changes (`mixer_vtol_transition_dynamic_mixer = ON`) use this progress: + +- MC -> FW: + - forward motor power ramps `0 -> 1` + - lift motor power ramps `1 -> vtol_transition_lift_end_percent` + - MC stabilisation ramps `1 -> vtol_transition_mc_authority_end_percent` + - FW control ramps `vtol_transition_fw_authority_start_percent -> 1` + +- FW -> MC: + - forward motor power ramps `1 -> 0` + - lift motor power ramps `vtol_transition_lift_end_percent -> 1` + - MC stabilisation ramps `vtol_transition_mc_authority_end_percent -> 1` + - FW control ramps `1 -> vtol_transition_fw_authority_start_percent` + +MC->FW uses separate forward-motor ramp-up and control handover behavior. +For MC->FW, forward motor power uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, the motor goes to full power immediately. +This timer does not decide when the transition completes. +Lift motor power, MC stabilisation, and FW control still prefer pitot-based transition progress whenever pitot is working. +If pitot stops being usable and `mixer_vtol_transition_scale_ramp_time_ms > 0`, those other changes fall back to the same timer-based ramp. +If pitot is not usable and `mixer_vtol_transition_scale_ramp_time_ms = 0`, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). +FW->MC keeps the existing style of smooth handover. + +For transition/pusher motors (`-2.0 < throttle < -1.0`), output is interpolated from idle to target: + +`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..f5cafa62ae7 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -103,7 +103,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 9); PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, @@ -117,6 +117,12 @@ 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, + .vtolFwToMcAutoSwitchAirspeed = SETTING_VTOL_FW_TO_MC_AUTO_SWITCH_AIRSPEED_CM_S_DEFAULT, + .vtolTransitionLiftEndPercent = SETTING_VTOL_TRANSITION_LIFT_END_PERCENT_DEFAULT, + .vtolTransitionMcAuthorityEndPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_END_PERCENT_DEFAULT, + .vtolTransitionFwAuthorityStartPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_START_PERCENT_DEFAULT, .craftName = SETTING_NAME_DEFAULT, .pilotName = SETTING_NAME_DEFAULT ); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index e3bde5f3eb7..1478754f350 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -78,6 +78,12 @@ 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; + uint16_t vtolFwToMcAutoSwitchAirspeed; + 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..e9045323abd 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 @@ -1272,11 +1281,33 @@ groups: field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." + description: "Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. If `mixer_vtol_transition_scale_ramp_time_ms = 0`, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 max: 200 + - name: mixer_vtol_transition_dynamic_mixer + description: "Turns on smooth VTOL transition power changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions." + default_value: OFF + field: mixer_config.vtolTransitionDynamicMixer + type: bool + - name: mixer_vtol_manualswitch_autotransition_controller + description: "Makes `MIXER TRANSITION` start one automatic VTOL transition each time the switch moves from OFF to ON, when not in waypoint mission. Turn this ON in both mixer profiles if you want the same behavior in both directions. OFF keeps the older manual switch behavior." + default_value: OFF + field: mixer_config.manualVtolTransitionController + type: bool + - name: mixer_vtol_transition_airspeed_timeout_ms + description: "Maximum wait time [ms] for the required pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, iNAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable." + default_value: 0 + field: mixer_config.vtolTransitionAirspeedTimeoutMs + min: 0 + max: 60000 + - name: mixer_vtol_transition_scale_ramp_time_ms + description: "When smooth VTOL transition power changes are ON, this always controls the MC->FW forward motor ramp. `0` gives full forward-motor power immediately. This timer does not decide when the transition is complete. For lift motor power, multicopter stabilisation, and fixed-wing control handoff, trusted pitot airspeed still controls the change while pitot is usable; this timer is only their backup ramp if pitot becomes unavailable." + 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 +2654,32 @@ groups: default_value: "RESUME" field: general.flags.waypoint_mission_restart table: nav_wp_mission_restart + - name: nav_vtol_mission_transition_user_action + description: "Chooses which waypoint USER flag (`USER1`..`USER4`) tells iNAV which flight mode to use at each navigable waypoint. Selected USER flag ON means fixed-wing. Selected USER flag OFF means multicopter. OFF disables this feature. Requires two mixer profiles and a working `MIXER PROFILE 2` mode setup." + default_value: "OFF" + field: general.vtol_mission_transition_user_action + table: nav_wp_user_action + - name: nav_vtol_mission_transition_min_altitude_cm + description: "Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the altitude check." + default_value: 0 + field: general.vtol_mission_transition_min_altitude + min: 0 + max: 50000 + - name: nav_vtol_transition_retry_on_airspeed_timeout + description: "If ON, iNAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more." + default_value: OFF + field: general.vtol_transition_retry_on_airspeed_timeout + type: bool + - name: nav_vtol_transition_fail_action_mc_to_fw + description: "What iNAV should do if MC->FW transition still fails after the final attempt." + default_value: "IDLE" + field: general.vtol_transition_fail_action_mc_to_fw + table: nav_vtol_transition_fail_action_mc_to_fw + - name: nav_vtol_transition_fail_action_fw_to_mc + description: "What iNAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met." + 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 +4014,42 @@ 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] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted." + default_value: 0 + field: vtolTransitionToFwMinAirspeed + min: 0 + max: 20000 + - name: vtol_transition_to_mc_max_airspeed_cm_s + description: "When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, iNAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted." + default_value: 0 + field: vtolTransitionToMcMaxAirspeed + min: 0 + max: 20000 + - name: vtol_fw_to_mc_auto_switch_airspeed_cm_s + description: "Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable." + default_value: 0 + field: vtolFwToMcAutoSwitchAirspeed + min: 0 + max: 20000 + - name: vtol_transition_lift_end_percent + description: "How much lift motor power remains at the end of transition, in percent. `100` keeps full lift power. Lower values reduce lift motor power more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + default_value: 100 + field: vtolTransitionLiftEndPercent + min: 0 + max: 100 + - name: vtol_transition_mc_authority_end_percent + description: "How much multicopter stabilisation remains at the end of transition, in percent. `100` keeps full multicopter stabilisation. Lower values reduce it more. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + default_value: 100 + field: vtolTransitionMcAuthorityEndPercent + min: 0 + max: 100 + - name: vtol_transition_fw_authority_start_percent + description: "How much fixed-wing control is available at the start of transition, in percent. `100` gives full fixed-wing control immediately. Lower values bring in fixed-wing control more gently. Used only when `mixer_vtol_transition_dynamic_mixer` is ON." + 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.h b/src/main/flight/mixer.h index 12688bd2c09..584c05b0888 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -43,6 +43,13 @@ typedef enum { PLATFORM_BOAT = 5 } flyingPlatformType_e; +static inline bool isMultirotorTypePlatform(const flyingPlatformType_e platformType) +{ + return platformType == PLATFORM_MULTIROTOR || + platformType == PLATFORM_TRICOPTER || + platformType == PLATFORM_HELICOPTER; +} + typedef enum { OUTPUT_MODE_AUTO = 0, diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index a39fbfeedd9..bdd322e3301 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,12 @@ bool isMixerTransitionMixing; bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; int nextMixerProfileIndex; +static bool manualTransitionModeWasActive; +static bool manualTransitionReadyForEdge = true; +static bool manualTransitionSessionLatched; +static bool manualFwToMcProtectionLatched; -PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 4); void pgResetFn_mixerProfiles(mixerProfile_t *instance) { @@ -53,6 +61,10 @@ 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, + .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,276 @@ 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.handoffScalingProgress = 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.handoffScalingProgress = 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.handoffScalingProgress = 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 getPusherRampProgress(void) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer) { + return 1.0f; + } + + if (currentMixerConfig.vtolTransitionScaleRampTimeMs <= 0) { + return 1.0f; + } + + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + return constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); +} + +static float getHandoffScalingProgress(void) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer) { + return 1.0f; + } + + if (mixerProfileAT.usedAirspeed) { + mixerProfileAT.handoffScalingProgress = constrainf(mixerProfileAT.progress, 0.0f, 1.0f); + return mixerProfileAT.handoffScalingProgress; + } + + if (currentMixerConfig.vtolTransitionScaleRampTimeMs > 0) { + const float rampProgress = getPusherRampProgress(); + + // Preserve already-applied handoff scaling if pitot drops out mid-transition. + mixerProfileAT.handoffScalingProgress = MAX(mixerProfileAT.handoffScalingProgress, rampProgress); + return mixerProfileAT.handoffScalingProgress; + } + + // Last-resort compatibility path: with no trusted pitot and no dedicated + // scaling ramp configured, reuse transition progress/timer behavior. + mixerProfileAT.handoffScalingProgress = MAX(mixerProfileAT.handoffScalingProgress, constrainf(mixerProfileAT.progress, 0.0f, 1.0f)); + return mixerProfileAT.handoffScalingProgress; +} + +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 bool hasPitotSensorForManualProtection(void) +{ +#ifdef USE_PITOT + if (!sensors(SENSOR_PITOT) || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE || + detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + return false; + } + + return true; +#else + return false; +#endif +} + +static uint16_t getAirspeedThresholdForDirection(const mixerProfileATDirection_e direction) +{ + if (direction == MIXERAT_DIRECTION_TO_FW) { + return systemConfig()->vtolTransitionToFwMinAirspeed; + } + + if (direction == MIXERAT_DIRECTION_TO_MC) { + return systemConfig()->vtolTransitionToMcMaxAirspeed; + } + + return 0; +} + +static bool shouldRequestManualFwToMcProtection(const bool manualControllerEnabled) +{ + if (!manualControllerEnabled || !STATE(AIRPLANE)) { + return false; + } + + const uint16_t thresholdCmS = systemConfig()->vtolFwToMcAutoSwitchAirspeed; + if (thresholdCmS == 0 || !hasPitotSensorForManualProtection()) { + return false; + } + + float airspeedCmS = 0.0f; + if (!hasTrustedPitotAirspeed(&airspeedCmS)) { + return false; + } + + return airspeedCmS <= thresholdCmS; +} + +static void updateTransitionScales(void) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer) { + 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 handoffProgress = getHandoffScalingProgress(); + + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + const float pusherProgress = getPusherRampProgress(); + + mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, pusherProgress); + mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, handoffProgress); + mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, handoffProgress); + mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, handoffProgress); + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, handoffProgress); + mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, handoffProgress); + mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, handoffProgress); + mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, handoffProgress); + } + + mixerProfileAT.blendToFw = constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); +} + +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 +400,16 @@ 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 isMultirotorTypePlatform(nextPlatformType); +} + bool checkMixerATRequired(mixerProfileATRequest_e required_action) { //return false if mixerAT condition is not required or setting is not valid @@ -132,17 +422,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 +453,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 +526,219 @@ 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; + const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; + const bool mixerProfileModePresent = isModeActivationConditionPresent(BOXMIXERPROFILE); + const int requestedProfileIndex = IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1; + const bool requestedMultirotorProfile = mixerProfileModePresent && + isMultirotorTypePlatform(mixerConfigByIndex(requestedProfileIndex)->platformType); + // If low-speed protection already moved the model back to MC, keep direct + // switching from forcing FW again until the pilot makes a new manual choice. + const bool fwToMcProtectionOwnsProfileSwitch = manualFwToMcProtectionLatched && + STATE(MULTIROTOR) && + !requestedMultirotorProfile; + + if (manualControllerConfigured && transitionModeRisingEdge) { + manualTransitionSessionLatched = true; + } + + if (transitionModeRisingEdge) { + manualFwToMcProtectionLatched = false; + } + + if (transitionModeFallingEdge) { + manualTransitionSessionLatched = false; + } + + if (requestedMultirotorProfile || (!mixerAT_inuse && !STATE(MULTIROTOR))) { + manualFwToMcProtectionLatched = false; + } + + if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { + abortTransition(false); + manualTransitionSessionLatched = false; + manualFwToMcProtectionLatched = false; + mixerAT_inuse = false; + } + + if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) { - if (isModeActivationConditionPresent(BOXMIXERPROFILE)){ - outputProfileHotSwitch(IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1); + if (mixerProfileModePresent && !transitionControllerOwnsProfileSwitch && !fwToMcProtectionOwnsProfileSwitch) { + outputProfileHotSwitch(requestedProfileIndex); + } + } + + // Recompute after a potential direct profile hot-switch because this flag is per-mixer-profile. + manualControllerEnabled = (currentMixerConfig.manualVtolTransitionController && !missionActive) || manualTransitionSessionLatched; + + if (!mixerAT_inuse && + shouldRequestManualFwToMcProtection(manualControllerEnabled) && + checkMixerATRequired(MIXERAT_REQUEST_MANUAL_TO_MC)) { + mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_MC); + mixerAT_inuse = mixerATIsActive(); + if (mixerAT_inuse || STATE(MULTIROTOR)) { + manualFwToMcProtectionLatched = true; + } + } + + 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(); + } } - isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); + 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(); } - isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); +} + +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..cbc69066ea0 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,6 +18,10 @@ typedef struct mixerConfig_s { bool controlProfileLinking; bool automated_switch; int16_t switchTransitionTimer; + 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 +38,58 @@ 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 handoffScalingProgress; + 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..7d7120dd85c 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,14 @@ #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 +#define NAV_MIXERAT_MISSION_TRANSITION_TRACK_DISTANCE_CM 4000 + /*----------------------------------------------------------- * Compatibility for home position *-----------------------------------------------------------*/ @@ -119,7 +128,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -149,6 +158,11 @@ 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_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,342 @@ 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 isMultirotorTypePlatform(platformType); +} + +#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; + calculateFarAwayTarget(&transitionTarget, navMixerATMissionTransition.heading, NAV_MIXERAT_MISSION_TRANSITION_TRACK_DISTANCE_CM); + setDesiredPosition(&transitionTarget, navMixerATMissionTransition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return; + } + + 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 +2396,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 +2719,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 +2728,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 +2741,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 +2757,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 +2847,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigatio { UNUSED(previousState); mixerATUpdateState(MIXERAT_REQUEST_ABORT); + clearMissionVTOLTransitionState(); return NAV_FSM_EVENT_SUCCESS; } @@ -5062,6 +5565,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..bff60d96d0e 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,11 @@ 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) + 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,