diff --git a/.gitignore b/.gitignore index 6f653fd9820..992c9348d04 100644 --- a/.gitignore +++ b/.gitignore @@ -52,6 +52,7 @@ tokens.yaml # Build output directories (catch-all for ad-hoc build dirs) /build_*/ +/build-*/ /build_hw/ # Local development files diff --git a/.vscode/tasks.json b/.vscode/tasks.json index b2b2c44ea0d..317f8774b5f 100755 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -4,19 +4,28 @@ "version": "2.0.0", "tasks": [ { - "label": "Build MATEKH743", + "label": "Install/Update CMAKE", "type": "shell", - "command": "ninja MATEKH743", + "command": "mkdir -p build && cd build && cmake ..", "group": "build", "problemMatcher": [], "options": { - "cwd": "${workspaceFolder}/build" + "cwd": "${workspaceFolder}" } }, { - "label": "Build MATEKH743", + "label": "Compile autogenerated docs", + "type": "shell", + "command": "python3 src/utils/update_cli_docs.py", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + } + }, + { + "label": "Build MAMBAH743_2022B", "type": "shell", - "command": "ninja MATEKH743", + "command": "make MAMBAH743_2022B", "group": { "kind": "build", "isDefault": true diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 685e17a4e70..6bf7f732cfc 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -6,21 +6,81 @@ A MixerProfile is a set of motor mixer, servo-mixer and platform type configurat Not limited to VTOL. air/land/sea mixed vehicle is also achievable with this feature. Model behaves according to current mixer_profile's platform_type and configured custom motor/servo mixer -Currently two profiles are supported on targets other than F411(due to resource constraints on F411). i.e VTOL transition is not available on F411. +Two mixer profiles and smooth VTOL auto-transition are available only on targets with enough flash space. +In standard INAV builds this means targets with more than 512 KB flash, compiled with `USE_AUTO_TRANSITION`. +Targets with 512 KB flash keep the older single-profile / legacy transition behavior and do not include the smooth auto-transition settings. For VTOL setup. one mixer_profile is used for multi-rotor(MR) and the other is used for fixed-wing(FW) By default, switching between profiles requires reboot to take affect. However, using the RC mode: `MIXER PROFILE 2` will allow in flight switching for things like VTOL operation . 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 the usable transition airspeed source gets too low. During manual FW flight, INAV stays in MC until you deliberately command another manual profile change. During mission/RTH/failsafe, INAV keeps the current navigation task in MC after the safety switch. + +This behavior is controlled by `mixer_vtol_manualswitch_autotransition_controller`. +Turn it ON in both mixer profiles if you want the same switch behavior in both directions. +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. +- During a smooth auto-transition, `MIXER TRANSITION` may remain active while INAV is still using the source profile, waiting for airspeed/timer completion, or finishing the safe output movement after the profile switch. +- During the same transition, `MIXER PROFILE 2` changes only when the actual active mixer profile changes. It is not a progress indicator. +- Mission, RTH, and LAND auto-transitions use the same internal transition state, so `MIXER TRANSITION` can appear active even when the RC `MIXER TRANSITION` switch was not used. Use the OSD system message or `DEBUG_VTOL_TRANSITION` when you need to see which internal transition state is active. + +There are two separate manual paths: + +- `MIXER PROFILE 2` is still a direct manual profile switch when `MIXER TRANSITION` is OFF. +- `MIXER TRANSITION` starts the smooth automatic transition sequence when `mixer_vtol_manualswitch_autotransition_controller = ON`. +- If both are ON together while the automatic transition controller is enabled, the controller temporarily owns the profile switching. When `MIXER TRANSITION` turns OFF again, direct `MIXER PROFILE 2` switching becomes active again. +- If low-speed protection switches the model from FW to MC, INAV keeps the MC profile until you deliberately command another manual profile change. + +3-position switch example: + +- 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` ON, `MIXER TRANSITION` ON) + - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) +- Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. +- If you intentionally swap the profile order, keep the same idea and swap the FW and MC end positions. -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 +Optional two-position desired-mode switch: + +- Set `vtol_autotransition_always = ON` if you want the profile switch to mean "go to this final mode". +- With this option enabled while armed, selecting FW starts MC->FW auto-transition instead of directly switching to FW. +- Selecting MC starts FW->MC auto-transition instead of directly switching to MC. +- If you move the switch back to the current profile before the profile switch happens, INAV cancels that transition and remains in the current profile. +- When disarmed, direct profile switching remains available for bench and preflight checks. +- Turn this option OFF if you want direct VTOL MC/FW profile switching while armed. +- Leave this option OFF if you want to keep the pilot choice between direct profile switching and a separate transition request. ## Servo @@ -50,13 +110,19 @@ Example: This will spin motor number 5 (counting from 1) at 20%, in transition m mmix 4 -1.200 0.000 0.000 0.000 ``` +For smooth VTOL auto-transition, prefer configuring the forward motor as a normal positive-throttle motor in the FW mixer profile and reserving the same motor index in the MC profile with a placeholder. Use `throttle = -1.000` as the placeholder if Configurator removes zero-throttle motor rules. Values below about `-1.05` are treated as the older transition helper motor style, not as a plain placeholder. + ## RC mode settings It is recommend that the pilot uses a RC mode switch to activate modes or switch profiles. 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,14 +132,296 @@ 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. + +For a simpler two-position workflow, set `vtol_autotransition_always = ON` and let the `MIXER PROFILE 2` switch select the desired final mode. In that setup the pilot does not need a middle transition position: switching toward FW starts MC->FW, and switching toward MC starts FW->MC. This option is global and changes armed manual VTOL switching only; disarmed direct profile switching remains available for bench and preflight checks. + ## 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. + +This section applies only to targets with more than 512 KB flash, compiled with `USE_AUTO_TRANSITION`. + +Direct manual `MIXER PROFILE 2` switching is still a separate path when you want an immediate profile change. + +The OSD system message field reports the internal transition state for manual, mission, RTH, LAND, retry, abort, timeout, and completion events. This is useful because Active Modes show RC mode/profile state, while the OSD message describes what the auto-transition controller is doing. + +### Airspeed-first completion + +When valid pitot airspeed is available, INAV uses airspeed to decide when the transition is complete: + +- `vtol_transition_to_fw_min_airspeed_cm_s` for MC->FW +- `vtol_transition_to_mc_max_airspeed_cm_s` for FW->MC + +If no usable transition airspeed source is available, or the threshold is set to `0`, INAV uses `mixer_switch_trans_timer` instead. A usable source is either a valid real pitot sensor or `pitot_hardware = VIRTUAL` with a valid virtual airspeed estimate. +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 a usable transition airspeed source is not being used. +- `mixer_vtol_transition_airspeed_timeout_ms` is only a maximum wait time for the required airspeed while the transition airspeed source is still usable. It does not complete the transition by itself; it aborts that airspeed-controlled attempt. +- If the airspeed source 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, you can configure `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules in the MC mixer profile. +During MC->FW they give those servos a preview of the fixed-wing stabilisation that will take over after the profile switch. +This preview uses the target fixed-wing PID bank, rates, angle limits, heading-hold limits, and turn-assist gains, but it still follows the current transition stick shaping until the actual profile switch. +During FW->MC the same MC mixer rules mark which FW servo outputs should reduce fixed-wing correction while motor stabilisation comes back in. +At the same time, target MC motor rules can come in before the switch and use a target MC PID preview, so lift motors are not driven by the active FW/PIFF controller. +These inputs are active only while the smooth auto-transition controller is running. If `mixer_vtol_transition_dynamic_mixer = OFF`, they stay at full authority while the controller is active. If `mixer_vtol_transition_dynamic_mixer = ON`, they follow the normal fixed-wing authority scaling. +`INPUT_MIXER_TRANSITION` remains available for tilt/helper servo movement. With `mixer_vtol_transition_dynamic_mixer = ON`, it follows `mixer_vtol_transition_scale_ramp_time_ms`. With it OFF, it keeps the older fixed transition behavior while the auto controller only decides the profile switch timing. If a profile switch or direct switch changes a transition-linked servo output, INAV starts a fresh full `mixer_vtol_transition_scale_ramp_time_ms` movement from the servo's current output instead of using a separate fixed servo delay. + +The smooth auto-transition controller owns the transition while it is running. It can therefore use target-profile transition rules even before the RC switch position would normally select that target profile. This is useful for FW->MC tilt-rotor setups where the FW profile has no tilt-servo rules, but the target MC profile has `INPUT_MIXER_TRANSITION` rules for those servo outputs. + +Forward motor setup for smooth auto-transition: + +- Preferred setup: configure the forward motor as a normal positive-throttle motor in the FW mixer profile. +- Reserve the same motor index in the MC mixer profile with a placeholder if that motor is not used in MC flight. Use `throttle = -1.000`, `roll = 0`, `pitch = 0`, `yaw = 0` if Configurator removes zero-throttle motor rules. +- During MC->FW, INAV can bring that target FW motor rule in before the profile switch. +- For FW->MC, configure the lift motors as normal positive-throttle motors in the MC mixer profile. If those motor indexes are not used in FW flight, reserve them in the FW mixer profile with placeholders. +- During FW->MC, INAV can bring those target MC lift motor rules in before the profile switch and use target MC stabilisation for their roll/pitch/yaw correction. If a target MC lift motor was idle in the FW profile, INAV captures the current real motor output and moves from that value toward the target MC lift output over `mixer_vtol_transition_scale_ramp_time_ms`. +- The older `-2.0 <= throttle <= -1.05` transition motor rule still works as a legacy/helper path, but it is not the preferred setup for smooth auto-transition. +- If a helper such as `throttle = -1.200` is used before the MC->FW switch, smooth auto-transition moves from that helper output to the real FW mixer output after the profile switch. +- If the same physical tilt motor is configured with positive throttle in both profiles on the same index, INAV blends the base throttle between the MC and FW mixer rules instead of adding both throttles together. The current-profile stabilisation is reduced while the target-profile stabilisation is increased. + +How `mixer_vtol_transition_scale_ramp_time_ms` works: + +- Time-based motor/power and transition-servo movement: + - MC->FW: forward motor power ramps from `0 -> 100%` over this time. + - After the MC->FW profile switch, lift motors that are not used by the FW profile move to idle over this same time instead of stopping immediately. + - FW->MC: forward motor power ramps from `100% -> 0%`, while lift power and MC stabilisation ramp from their configured minimums back to `100%` over this time. + - FW->MC target lift motors that exist only in the MC profile start from their current real output and move toward the target MC lift output over this time. + - After the FW->MC profile switch, a forward motor that is not used by the MC profile keeps moving to idle and cannot be reintroduced by FW throttle. + - `INPUT_MIXER_TRANSITION` uses this ramp when `mixer_vtol_transition_dynamic_mixer = ON`; with dynamic mixer OFF it keeps the older fixed transition endpoint behavior. + - If a profile switch or direct switch changes a transition-linked servo output, INAV captures the current servo output and starts a fresh full window from this value. + - `= 0` (default): those time-based power changes happen immediately. +- For FW->MC tilt servos that exist only in the target MC profile, INAV can preview only the target `INPUT_MIXER_TRANSITION` plus `INPUT_MAX` movement from the servo's current real output. This means it adds the target transition movement and the target fixed offset when both rules exist. A `MAX` rule alone is not enough to identify a transition servo. Target yaw/roll/pitch stabilisation is not applied until the MC profile is active. +- This timer does not decide when the transition completes. +- Airspeed-based control scaling: + - in MC->FW, lift power reduction, MC stabilisation reduction, and FW control increase follow airspeed when pitot is usable. + - in FW->MC, FW control reduction follows airspeed when pitot is usable. + - with valid pitot airspeed, they follow transition progress based on airspeed. + - without a usable transition airspeed source, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). +- In FW->MC, forward motor removal, lift power return, and MC motor stabilisation return use the time-based motor ramp so they do not wait for airspeed to fall first. + +`vtol_transition_lift_min_percent` and `vtol_transition_mc_authority_min_percent` control different parts of the MC motor output: + +- `vtol_transition_lift_min_percent` scales the lift motor base throttle. +- `vtol_transition_mc_authority_min_percent` scales the MC roll/pitch/yaw stabilisation correction mixed into those motors. + +Pilot throttle still matters. In ALTHOLD/NAV, `nav_mc_manual_climb_rate` limits the climb/descent rate requested by the throttle stick. In pure manual throttle modes, the transition controller does not automatically convert a strong manual climb command into hover throttle, so avoid entering MC->FW while commanding an aggressive vertical climb. + +Example: -## 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. +- `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`, +- in FW->MC, the forward motor ramps down while lift power and MC stabilisation return in about `1.2s`, +- when pitot is working, airspeed-linked control scaling still follows airspeed, +- if the airspeed source stops being usable, airspeed-linked control scaling falls back to `mixer_switch_trans_timer`, +- transition completion still uses airspeed when a usable transition airspeed source is available, +- backup completion time is still `5s` if no usable transition airspeed source is available. + +### 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_min_percent` + - `vtol_transition_mc_authority_min_percent` + - `vtol_transition_fw_authority_min_percent` + - `nav_vtol_mission_transition_user_action` + - `nav_vtol_mission_transition_min_altitude_cm` + +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_min_percent = 30` +- `set vtol_transition_mc_authority_min_percent = 20` +- `set vtol_transition_fw_authority_min_percent = 20` +- `set nav_vtol_mission_transition_user_action = OFF` + +Behavior: +- Uses pitot airspeed first, with timer fallback only if pitot is not usable. +- Adds smoother forward-motor, lift-motor, and control changes 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_min_percent = 30` +- `set vtol_transition_mc_authority_min_percent = 20` +- `set vtol_transition_fw_authority_min_percent = 20` +- `set nav_vtol_mission_transition_user_action = USER1` +- `set nav_vtol_mission_transition_min_altitude_cm = 1200` + +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`, `3=after-switch smoothing`) +- `debug[1]` = active request (`MIXERAT_REQUEST_*` enum value), with transition direction packed in bits `8..15` and wait reason packed in bits `16..23` + - wait reason `0`: none + - wait reason `1`: FW->MC is close to the MC speed threshold, but is still waiting + - wait reason `2`: no usable transition airspeed source is available, so timer-based completion is being used + - wait reason `3`: FW->MC is waiting because the usable airspeed source is still clearly above `vtol_transition_to_mc_max_airspeed_cm_s` + +For display only, reason `3` means the usable airspeed value is more than about `1 m/s` above the MC switch threshold. The actual FW->MC switch condition is still `airspeed <= vtol_transition_to_mc_max_airspeed_cm_s`. +- `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 + - bit20: manual transition session is latched + - bits 21-24: active platform type + - bits 25-26: active PID type (`pidIndexGetType(PID_ROLL)`) + - bits 27-28: target preview mode (`0=none`, `1=manual FW`, `2=target FW PID`, `3=target MC PID`) + - bit29: after-switch smoothing active + - bit30: legacy manual transition session active +- `debug[3]` = progress x1000 (`0..1000`) +- `debug[4]` = pusher scale x1000 (`0..1000`) +- `debug[5]` = lift scale x1000 (`0..1000`) +- `debug[6]` = packed scales: + - low 16 bits: MC stabilisation scale x1000 (`0..1000`) + - high 16 bits: FW control scale x1000 (`0..1000`) +- `debug[7]` = packed progress values: + - bits 0-9: progress used for airspeed-linked scaling (`0..1000`) + - bits 10-19: motor ramp progress (`0..1000`) + - bits 20-29: after-switch smoothing progress (`0..1000`) + +Final motor outputs are available in the normal Blackbox motor traces. During after-switch smoothing, the old lift or pusher motor output should move smoothly toward idle there. + +## TailSitter +Configure the fixed-wing mixer profile normally with `set platform_type = AIRPLANE`; the motor thrust axis should point in the same direction as the airplane nose. Configure the takeoff and landing profile as a normal multirotor profile, then set `tailsitter_orientation_offset = ON`. There is no separate `TAILSITTER` platform type. The orientation offset changes the active board/mixer orientation for the multicopter profile. While `MIXER TRANSITION` is active, INAV also applies the tailsitter's 45-degree transition target in ANGLE mode. ## Parameter list (Partial List) #### Please be aware of what parameter is shared among FW/MC modes and what isn't. @@ -107,7 +455,7 @@ TailSitter is supported by add a 90deg offset to the board alignment. Set the bo - ······· ### TailSitter support -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 platform_type = TAILSITTER`. The `TAILSITTER` platform type is same as `MULTIROTOR` platform type, expect for a 90 deg board alignment offset. In `TAILSITTER` mixer_profile, when motor trust/airplane nose is pointing to the sky, 'airplane bottom'/'multi rotor front' should facing forward in model preview. Set the motor/servo mixer according to multirotor orientation, Model should roll around geography's longitudinal axis, the roll axis of `TAILSITTER` will be yaw axis of `AIRPLANE`. In addition, When `MIXER TRANSITION` input is activated, a 45deg offset will be add to the target angle for angle mode. +TailSitter uses two ordinary mixer profiles. The fixed-wing profile uses `set platform_type = AIRPLANE`, with the motor thrust axis pointing in the same direction as the airplane nose. The takeoff/landing profile uses a multirotor `platform_type` and `set tailsitter_orientation_offset = ON`; there is no `TAILSITTER` platform type. When the motor thrust/airplane nose points upward in the multicopter profile, the airplane bottom/multirotor front should face forward in the Configurator model preview. Configure motors and servos for that multirotor orientation: the aircraft's fixed-wing roll axis becomes the multicopter yaw axis. When `MIXER TRANSITION` is active, INAV adds the 45-degree tailsitter transition target for ANGLE mode. ## Happy flying diff --git a/docs/Navigation.md b/docs/Navigation.md index 4d7b9740d91..35135e744a1 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -102,6 +102,106 @@ Parameters: * `` - Last waypoint must have `flag` set to 165 (0xA5). +### Mission VTOL transition using existing User Actions + +Mission VTOL transition can be requested. +This is available only on targets with more than 512 KB flash, compiled with `USE_AUTO_TRANSITION`. +Targets with 512 KB flash do not include these mission VTOL transition settings. + +Configuration: + +- `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. +- If an airspeed-controlled MC -> FW transition times out, `nav_vtol_transition_retry_on_airspeed_timeout` can run one heading scan/retry before the configured fail action is used. + +Safety and scope: + +- This path uses authorized automated transition state handling; it does not permit manual mixer profile switching during normal waypoint navigation. +- It still depends on valid mixer profile switching infrastructure (two configured mixer profiles and a valid `MIXER PROFILE 2` mode activation condition). + +RTH and failsafe VTOL transitions: + +- RTH may request MC -> FW before flying home if the aircraft is in MC and far enough from home. +- RTH landing may request FW -> MC before using the MC landing controller. +- Failsafe RTH/LAND is allowed to continue those navigation-owned `RTH` and `LAND` transition requests. +- `vtol_fw_to_mc_auto_switch_airspeed_cm_s` can also request a navigation-owned FW -> MC safety transition during mission, RTH, or failsafe RTH when trusted pitot airspeed falls too low. +- This low-speed safety transition requires `mixer_automated_switch = ON` and a valid MC target profile. +- After the low-speed safety transition switches to MC, INAV keeps the current navigation task in MC and blocks automatic MC -> FW RTH or mission re-entry for that navigation session. +- Manual and mission transition requests are not allowed to continue just because failsafe became active; they are aborted unless the target profile has already been selected and INAV is only finishing the remaining safe output movement. +- `vtol_transition_to_mc_max_airspeed_cm_s` controls when an already-requested FW -> MC transition is considered safe to complete. + +### VTOL MC navigation protection and landing detection + +Targets with more than 512 KB flash can enable extra protection for VTOL aircraft flying in MC mode: + +- `vtol_mc_protection_mode = OFF`: disabled, legacy behavior. +- `vtol_mc_protection_mode = NAV`: protects VTOL MC navigation and altitude-control behavior. +- `vtol_mc_protection_mode = NAV_AND_STABILIZED`: also shapes ANGLE/HORIZON roll, pitch, and yaw commands at higher horizontal speed. + +The protection only activates when the current mixer profile is multicopter-like and another configured mixer profile is fixed-wing. Normal multirotors and fixed-wing mode are not changed. + +In NAV modes, VTOL MC protection adds: + +- throttle reserve before altitude PID anti-windup, controlled by `vtol_mc_thr_reserve_percent`, +- capture/settle when entering position-holding navigation with horizontal speed, +- soft altitude capture while horizontal speed is being bled off, +- a stricter RTH/WP landing settle gate before descent starts, +- a conservative bailout path if attitude becomes excessive while automatic throttle is active. + +The landing settle gate uses `min(nav_wp_radius, 100 cm)` as the capture radius for the landing point. It also requires low horizontal speed, low vertical speed, and safe attitude to be held for the internal settle time. This prevents a large `nav_wp_radius` from starting landing descent after only briefly touching the waypoint radius while still moving. + +ANGLE/HORIZON shaping in `NAV_AND_STABILIZED` only runs when armed, VTOL MC mode is detected, velocity estimate is trusted, and horizontal speed is above the shaping threshold. It continuously scales roll, pitch, and yaw commands as speed increases, preserving command sign and small deadband behavior. + +#### Landing detector sensitivity + +`nav_land_detect_sensitivity` scales the generic landing detector velocity and gyro thresholds. The default `5` is nominal sensitivity. For multirotors, this corresponds to about: + +- `100 cm/s` horizontal speed, +- `100 cm/s` vertical speed, +- `4 deg/s` average pitch/roll gyro rate. + +Higher values relax these thresholds and can make landing detection faster, but also increase false-detect risk. VTOL MC landing detection adds additional safety gates that `nav_land_detect_sensitivity` does not bypass: + +- vertical speed must be near zero when altitude/vertical-speed estimate is available, +- NAV landing must be in the final slow-descent context, +- trusted surface/AGL data, if available, must show near-ground, +- all VTOL MC landing candidates must pass a throttle-probe confirmation before `LANDING_DETECTED`. + +The VTOL MC throttle probe gently reduces lift throttle for a short confirmation window. If the aircraft starts descending, shows unloading acceleration, or trusted AGL drops, the candidate is rejected and the detector waits again. This is intended to avoid false disarm during slow descent or ground-effect bounce, without relying on barometric altitude as AGL. + +`nav_landing_bump_detection = ON` allows G-bump touchdown detection to create a landing candidate. For VTOL MC it is not an immediate disarm shortcut: trusted high AGL blocks it, and accepted candidates still go through the throttle probe. For non-VTOL multirotors it keeps the existing landing detector behavior. + +Automatic disarm still requires `nav_disarm_on_landing = ON`. `nav_auto_disarm_delay` is applied after a landing candidate is detected; in VTOL MC mode the throttle probe must also confirm before the global `LANDING_DETECTED` state is set. + +Debugging: + +- `debug_mode = VTOL_MC_PROTECT` shows protection flags, safe throttle range, protected throttle, speed, attitude, and settle/command-scale progress. +- `debug_mode = LANDING` shows normal landing detector status and candidate state. + `wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`). `wp reset` - Resets the list, sets the number of waypoints to 0 and marks the list as invalid (but doesn't delete the waypoint definitions). diff --git a/docs/Settings.md b/docs/Settings.md index 243314fc6d8..b232eb48c3f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -794,6 +794,8 @@ Defines debug values exposed in debug variables (developer / debugging setting) | LULU | | | SBUS2 | | | OSD_REFRESH | | +| VTOL_TRANSITION | | +| VTOL_MC_PROTECT | | --- @@ -2905,9 +2907,9 @@ Autopilot type to advertise for MAVLink telemetry --- -### mavlink_ext_status_rate +### mavlink_port1_ext_status_rate -Rate of the extended status message for MAVLink telemetry +Rate of the extended status message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2915,9 +2917,9 @@ Rate of the extended status message for MAVLink telemetry --- -### mavlink_extra1_rate +### mavlink_port1_extra1_rate -Rate of the extra1 message for MAVLink telemetry +Rate of the extra1 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2925,9 +2927,9 @@ Rate of the extra1 message for MAVLink telemetry --- -### mavlink_extra2_rate +### mavlink_port1_extra2_rate -Rate of the extra2 message for MAVLink telemetry +Rate of the extra2 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2935,9 +2937,9 @@ Rate of the extra2 message for MAVLink telemetry --- -### mavlink_extra3_rate +### mavlink_port1_extra3_rate -Rate of the extra3 message for MAVLink telemetry +Rate of the extra3 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2945,9 +2947,19 @@ Rate of the extra3 message for MAVLink telemetry --- -### mavlink_min_txbuffer +### mavlink_port1_high_latency -Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. +Enable MAVLink high-latency mode on port 1 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port1_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 1. Requires RADIO_STATUS messages. | Default | Min | Max | | --- | --- | --- | @@ -2955,9 +2967,9 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry --- -### mavlink_pos_rate +### mavlink_port1_pos_rate -Rate of the position message for MAVLink telemetry +Rate of the position message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2965,21 +2977,22 @@ Rate of the position message for MAVLink telemetry --- -### mavlink_radio_type +### mavlink_port1_radio_type -Mavlink radio type. Affects how RSSI and LQ are reported on OSD. +MAVLink radio type for port 1. Affects RSSI and LQ reporting on OSD. | Allowed Values | | | --- | --- | | GENERIC | Default | | ELRS | | | SIK | | +| MLRS | | --- -### mavlink_rc_chan_rate +### mavlink_port1_rc_chan_rate -Rate of the RC channels message for MAVLink telemetry +Rate of the RC channels message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2987,6 +3000,105 @@ Rate of the RC channels message for MAVLink telemetry --- +### mavlink_port2_high_latency + +Enable MAVLink high-latency mode on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port2_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 2. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port2_radio_type + +MAVLink radio type for port 2. Affects RSSI and LQ reporting on OSD. + +| Allowed Values | | +| --- | --- | +| GENERIC | Default | +| ELRS | | +| SIK | | +| MLRS | | + +--- + +### mavlink_port3_high_latency + +Enable MAVLink high-latency mode on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port3_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 3. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port3_radio_type + +MAVLink radio type for port 3. Affects RSSI and LQ reporting on OSD. + +| Allowed Values | | +| --- | --- | +| GENERIC | Default | +| ELRS | | +| SIK | | +| MLRS | | + +--- + +### mavlink_port4_high_latency + +Enable MAVLink high-latency mode on port 4 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port4_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 4. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port4_radio_type + +MAVLink radio type for port 4. Affects RSSI and LQ reporting on OSD. + +| Allowed Values | | +| --- | --- | +| GENERIC | Default | +| ELRS | | +| SIK | | +| MLRS | | + +--- + ### mavlink_sysid MAVLink System ID @@ -3241,7 +3353,7 @@ This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly ### mixer_automated_switch -If set to on, This mixer_profile will try to switch to another mixer_profile when 1.RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius on mixer_profile is a MULTIROTOR or TRICOPTER platform_type. 2. RTH landing is requested on this mixer_profile is a AIRPLANE platform_type +Allows navigation code to request VTOL mixer profile changes from this profile. In legacy RTH this lets MC/TRICOPTER profiles switch toward FW when returning from far away, and lets AIRPLANE profiles switch toward MC for RTH landing. With the VTOL auto-transition controller, mission waypoints can also request MC/FW transitions through their USER action. Enable this in each profile where NAV is allowed to change the mixer profile. | Default | Min | Max | | --- | --- | --- | @@ -3261,7 +3373,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 a usable transition airspeed source is not available, INAV completes the transition from this timer instead. A usable transition airspeed source is a valid real pitot sensor, or `pitot_hardware = VIRTUAL` with a valid virtual airspeed estimate. With smooth VTOL transition power changes ON, airspeed-linked power and control changes also fall back to this timer whenever the transition airspeed source is not usable. | Default | Min | Max | | --- | --- | --- | @@ -3269,6 +3381,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. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mixer_vtol_transition_airspeed_timeout_ms + +Maximum wait time [ms] for the required airspeed during an airspeed-controlled transition. The airspeed condition must be reached and remain satisfied for the internal 300 ms confirmation within this total time. This timer does not complete the transition; it only aborts it if the condition is not confirmed in time while the transition airspeed source remains usable. Real pitot and explicitly configured `pitot_hardware = VIRTUAL` can both be used. A source dropout shorter than 300 ms is ignored; persistent loss falls back to `mixer_switch_trans_timer`. Set to 0 to disable. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + +### mixer_vtol_transition_dynamic_mixer + +Turns on smooth VTOL transition power and control scaling. In MC->FW it smoothly brings the forward motor in while lift power and multicopter stabilisation are reduced and fixed-wing control is increased. In FW->MC it smoothly removes the forward motor while target MC lift power and target MC motor stabilisation come back in, and fixed-wing control is reduced. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mixer_vtol_transition_scale_ramp_time_ms + +Controls time-based smooth VTOL motor, power, and transition-servo movement. In MC->FW it moves the forward motor from idle to the requested power. After the switch to FW, old lift motors that are no longer used by the FW profile are moved to idle over the same time. In FW->MC it moves the forward motor down to idle while target MC lift power plus target MC motor stabilisation come back in. `INPUT_MIXER_TRANSITION` also uses this time when `mixer_vtol_transition_dynamic_mixer` is ON. If a profile switch or direct switch changes a transition-linked servo output, INAV starts a fresh smooth movement from the servo's current output using this full time value. `0` applies these time-based changes immediately. This timer does not decide when transition completes; completion still uses a usable transition airspeed source or `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash. + +| 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. @@ -4061,7 +4213,7 @@ Deadband for heading trajectory PID controller. When heading error is below the ### nav_land_detect_sensitivity -Changes sensitivity of landing detection. Higher values increase speed of detection but also increase risk of false detection. Default value should work in most cases. +Scales the generic landing detector velocity and gyro thresholds. `5` is nominal sensitivity: for MC this is about 100 cm/s horizontal and vertical speed plus 4 deg/s average pitch/roll gyro rate. Higher values relax these thresholds and can detect landing sooner, but increase false-detect risk. VTOL MC landing detection also uses additional hard vertical-speed and throttle-probe safety gates that this setting does not bypass. | Default | Min | Max | | --- | --- | --- | @@ -4111,7 +4263,7 @@ Defines at what altitude the descent velocity should start to be `nav_land_minal ### nav_landing_bump_detection -Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS). +Allows landing detection to use a touchdown G-bump candidate when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS). For VTOL MC mode the G-bump is not an immediate disarm shortcut: trusted high AGL blocks it, and accepted candidates must still pass the VTOL MC throttle-probe confirmation. | Default | Min | Max | | --- | --- | --- | @@ -4716,9 +4868,70 @@ 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. Available only on targets with more than 512 KB flash. + +| 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, a working `MIXER PROFILE 2` mode setup, and a target with more than 512 KB flash. + +| 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. Available only on targets with more than 512 KB flash. + +| 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. Available only on targets with more than 512 KB flash. + +| 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. Available only on targets with more than 512 KB flash. + +| 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. +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. After a successful mission VTOL transition from an ordinary WAYPOINT, a temporary transition-induced climb does not require loitering back down; the mission advances unless the aircraft is below the target altitude minus this tolerance. HOLD_TIME and LAND waypoints keep their normal behavior. | Default | Min | Max | | --- | --- | --- | @@ -7076,6 +7289,98 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, --- +### vtol_autotransition_always + +When enabled, an armed manual VTOL profile switch between a multicopter-like profile and a fixed-wing profile starts the auto-transition controller instead of switching profiles directly. This allows a two-position desired-mode switch: selecting FW starts MC->FW, selecting MC starts FW->MC. Disarmed direct profile switching remains available for bench and preflight checks. OFF preserves direct manual profile switching when `MIXER TRANSITION` is not active. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### vtol_fw_to_mc_auto_switch_airspeed_cm_s + +Extra low-speed fixed-wing safety fallback [cm/s]. If a usable transition airspeed source falls to this value or lower while in FW, INAV automatically starts FW->MC. Real pitot and explicitly configured `pitot_hardware = VIRTUAL` can both be used. In manual FW flight this requires `mixer_vtol_manualswitch_autotransition_controller` to be ON and keeps MC until the pilot deliberately commands another manual profile change. In mission/RTH/failsafe this requires `mixer_automated_switch` to be ON and keeps the current navigation task in MC after the safety switch, blocking automatic MC->FW re-entry for that navigation session. `vtol_transition_to_mc_max_airspeed_cm_s` still controls when the FW->MC transition is safe to complete. Set to 0 to disable. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20000 | + +--- + +### vtol_mc_protection_mode + +Enables extra protection for VTOL aircraft while the active mixer profile is multicopter-like and another mixer profile is fixed-wing. OFF keeps legacy behavior. NAV protects altitude/NAV throttle authority, NAV capture/settle, landing settle, VTOL MC landing confirmation, and conservative bailout. NAV_AND_STABILIZED also applies speed-based roll/pitch/yaw command shaping in ANGLE/HORIZON when armed, VTOL MC mode is detected, velocity is trusted, and horizontal speed is high enough. Available only on targets with more than 512 KB flash. + +| Allowed Values | | +| --- | --- | +| OFF | Default | +| NAV | | +| NAV_AND_STABILIZED | | + +--- + +### vtol_mc_thr_reserve_percent + +Throttle range percentage reserved on both low and high side for VTOL MC attitude authority while navigation/altitude control owns throttle. The reserve is applied before altitude PID anti-windup bounds and is automatically shrunk if needed to keep hover throttle inside the usable range. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 0 | 40 | + +--- + +### vtol_transition_fw_authority_min_percent + +Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength as the transition progresses. In FW->MC, it is reduced from full strength to this value as the transition progresses. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_lift_min_percent + +Lowest lift motor base throttle used during transition, in percent. This scales the throttle/lift component of MC lift motors, not their roll/pitch/yaw stabilisation correction. In MC->FW, lift motor base throttle is reduced to this value as the transition progresses. In FW->MC, target MC lift motor base throttle starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift base throttle through the whole transition, but also starts FW->MC target lift at full base throttle. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_mc_authority_min_percent + +Lowest multicopter stabilisation authority used during transition, in percent. This scales the MC roll/pitch/yaw correction mixed into MC motors, not the lift motor base throttle. In MC->FW, active MC motor stabilisation correction is reduced to this value as the transition progresses. In FW->MC, target MC motor stabilisation correction starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation correction through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_to_fw_min_airspeed_cm_s + +Minimum airspeed [cm/s] needed before MC->FW transition is considered complete while a usable transition airspeed source is available. The speed must remain at or above this value for the internal 300 ms confirmation. A usable source is a valid real pitot sensor, or `pitot_hardware = VIRTUAL` with a valid virtual airspeed estimate. If no usable source is available, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If the source remains usable but this target is not confirmed before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20000 | + +--- + +### vtol_transition_to_mc_max_airspeed_cm_s + +When slowing down from FW to MC, the transition is considered complete once airspeed remains at this value [cm/s] or lower for the internal 300 ms confirmation while a usable transition airspeed source is available. A usable source is a valid real pitot sensor, or `pitot_hardware = VIRTUAL` with a valid virtual airspeed estimate. If no usable source is available, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If the source remains usable but this condition is not confirmed before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 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 403e80d21e7..f31fa1a30fa 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. @@ -54,7 +60,7 @@ set motor_pwm_protocol = DSHOT300 #Try dshot first and see if it works set airmode_type = STICK_CENTER_ONCE -set nav_disarm_on_landing = OFF #band-aid for false landing detection in NAV landing of multi-copter +set nav_disarm_on_landing = OFF #Recommended for first VTOL tests; enable only after validating landing detection on your airframe set nav_rth_allow_landing = FS_ONLY set nav_wp_max_safe_distance = 500 set nav_fw_control_smoothness = 2 @@ -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,9 +153,9 @@ You must also assign the tilting servos values using the Fixed Value values (for 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. - - Utilize the Fixed Value values (formerly called "MAX") input in the servo mixer to tilt the motors without altering the servo midpoint. +2. **Configure the multicopter/tricopter:** + - Set up your multicopter/tricopter as usual, this time for mixer_profile 2 and control_profile 2. + - Utilize the Fixed Value input (formerly called "MAX") 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 - compass is required to enable navigation modes for multi-rotor profile. @@ -158,12 +164,29 @@ You must also assign the tilting servos values using the Fixed Value values (for ![Alt text](Screenshots/mixerprofile_mc_mixer.png) -5. **Tailsitters:planned for INAV 7.1** +5. **Tailsitters** - Configure the fixed-wing mode/profile sets normally. Use MultiCopter platform type for tail_sitting flying mode/profile sets. - The baseline board aliment is FW mode (ROLL axis is the thrust axis). Set `tailsitter_orientation_offset = ON ` in the tail_sitting MC mode. - Configure mixer ROLL/YAW mixing according to tail_sitting orientation in the tail_sitting MC mode. YAW axis is the thrust axis. - Conduct a bench test and see the orientation of the model changes in inav-configurator setup tab +### Experimental manual tailsitter auto-transition + +The first auto-transition implementation for a tailsitter is deliberately limited to manual switch requests. It is intended for supervised bench and initial flight testing only. It does not change the tilt-motor or pusher transition paths. + +Use the normal two-profile arrangement: + +- **FW profile:** `platform_type = AIRPLANE`, with `tailsitter_orientation_offset = OFF`. +- **MC profile:** a multirotor platform type, with `tailsitter_orientation_offset = ON`. +- Enable `mixer_vtol_manualswitch_autotransition_controller = ON` in both profiles. +- Keep `mixer_vtol_transition_dynamic_mixer = OFF` in both profiles. Dynamic motor/authority blending is not validated for a tailsitter yet. + +For a FW-to-MC request, configure a non-zero `vtol_transition_to_mc_max_airspeed_cm_s` and use a valid airspeed source. Real pitot and a valid virtual pitot are accepted. The controller does **not** use `mixer_switch_trans_timer` as a FW-to-MC fallback for tailsitters: changing a tailsitter into MC orientation at unknown airspeed is unsafe. If valid airspeed is not available, OSD shows `VTOL NO SPEED`; after `mixer_vtol_transition_airspeed_timeout_ms`, the transition aborts and the aircraft remains in FW. + +When measured airspeed is at or below the configured MC limit, INAV switches to the MC profile and keeps the existing 45 degree tailsitter transition pitch target. OSD shows `VTOL TAILSITTER CAPTURE` until the MC-orientation pitch estimate stays within 35 to 55 degrees for 300 ms. It then clears the transition and reports completion. If the pilot moves the switch back to the FW endpoint during this capture phase, INAV starts the ordinary MC-to-FW transition instead of direct-switching the profile. + +This first version intentionally does not support tailsitter auto-transition from missions, RTH, landing, failsafe, low-FW-airspeed protection, or VTOL MC protection. Those paths are rejected for the canonical tailsitter profile pair rather than running the unvalidated generic auto-transition sequence. Test without propellers first, verify the orientation change in the Configurator, and inspect a `DEBUG_VTOL_TRANSITION` log before attempting a flight. + # STEP 3: Mode Tab Settings: ### We recommend using an 3-pos switch on you radio to activate these modes, So the pilot can jump in or bail out at any moment. @@ -174,6 +197,12 @@ You must also assign the tilting servos values using the Fixed Value values (for | :-- | :-- | :-- | | 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 +268,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. @@ -247,7 +276,7 @@ Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weigh ## Motor 'Transition Mixing': Dedicated forward motor configuration In motor mixer set: -- -2.0 < throttle < -1.0: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated. +- -2.0 <= throttle <= -1.05: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated. Use exactly `-1.000` only as a placeholder, not as a transition helper motor. - Airmode type should be set to "STICK_CENTER". Airmode type must NOT be set to "THROTTLE_THRESHOLD". If set to throttle threshold the (-) motor will spin until the throttle threshold is passed. ![Alt text](Screenshots/mixerprofile_4puls1_mix.png) @@ -257,32 +286,1006 @@ No additional settings needed, 45 deg offset will be added to target pitch angle ### With aforementioned settings, your model should be able to enter fixed-wing profile without stalling. -# Automated Switching (RTH) (Optional): -### This is one of the least tested features. This feature is primarily designed for Return to Home (RTH) in the event of a failsafe. -When configured correctly, the model will use the Fixed-Wing (FW) mode to efficiently return home and then transition to Multi-Copter (MC) mode for easier landing. +# Smooth VTOL Auto-Transition Setup + +This section describes the new VTOL auto-transition features as a practical setup path. It is written as a sequence of small steps: + +1. Manual switch auto transition +2. Manual switch auto transition with dynamic scaling +3. Automated mission transition +4. Adding VTOL MC stabilisation protection +5. Landing detection setup + +The examples assume: + +- mixer profile 1 is the fixed-wing profile (`FW`, usually `platform_type = AIRPLANE`) +- mixer profile 2 is the multicopter profile (`MC`, for example `MULTIROTOR` or `TRICOPTER`) +- `MIXER PROFILE 2` mode selects the MC profile when you fly manually +- the model has been bench-tested without propellers before flight + +The smooth auto-transition controller is available only on targets with more than 512 KB flash. Smaller targets keep the older VTOL transition behavior and do not include these new settings. + +### Fixed-wing Auto Speed mode is separate from VTOL transition + +`AUTO SPEED` is an optional fixed-wing mode. It is not a VTOL transition setting and it never decides when an MC -> FW or FW -> MC transition starts or completes. + +When the aircraft is in a normal fixed-wing flight state and `AUTO SPEED` is enabled, INAV uses a separate PID controller to adjust throttle toward the speed requested by `fw_auto_speed_channel`. With a valid pitot source it controls airspeed. Without a valid pitot source it controls ground speed and keeps a conservative minimum throttle that is adjusted for pitch, reducing the chance of a downwind stall. + +**INAV intentionally disables Auto Speed for the entire time that the VTOL auto-transition controller owns the transition.** This applies to manual, mission, RTH, failsafe, retry, abort, and post-switch completion phases. The transition's own pusher, lift, motor-authority, and servo logic remains the only logic that can change transition power or control scaling. + +After the controller has finished and the aircraft is in its normal FW navigation or flight state, Auto Speed can become active again if its RC mode is enabled and its usual sensor and safety conditions are met. Leave the `AUTO SPEED` mode disabled while validating a new VTOL configuration. Test it separately in stable FW flight before deliberately combining it with an established VTOL mission. + +## 1. Manual switch auto transition -To enable this feature, type following command in cli +Manual switch auto transition means that `MIXER TRANSITION` starts one complete transition. You no longer need to manually time the exact profile switch point. INAV starts the transition, waits for the configured speed or timer condition, then changes to the target mixer profile. + +This does not remove the older behavior. If `mixer_vtol_manualswitch_autotransition_controller = OFF`, manual transition stays as close as possible to the older INAV behavior. + +### Recommended first setup + +Configure both mixer profiles. This makes MC -> FW and FW -> MC behave consistently. + +Per-mixer-profile settings: + +``` +mixer_profile 1 +set mixer_vtol_manualswitch_autotransition_controller = ON +set mixer_vtol_transition_dynamic_mixer = OFF +set mixer_switch_trans_timer = 50 +set mixer_vtol_transition_airspeed_timeout_ms = 0 +set mixer_vtol_transition_scale_ramp_time_ms = 0 + +mixer_profile 2 +set mixer_vtol_manualswitch_autotransition_controller = ON +set mixer_vtol_transition_dynamic_mixer = OFF +set mixer_switch_trans_timer = 50 +set mixer_vtol_transition_airspeed_timeout_ms = 0 +set mixer_vtol_transition_scale_ramp_time_ms = 0 +``` + +Global settings: -1. In your MC mode mixer profile (e.g., mixer_profile 2), set `mixer_automated_switch` to `ON`. leave it to `OFF` if burning remaining battery capacity on the way home is acceptable. ``` -mixer_profile 2or1 -set mixer_automated_switch= ON +set vtol_transition_to_fw_min_airspeed_cm_s = 0 +set vtol_transition_to_mc_max_airspeed_cm_s = 0 +set nav_vtol_mission_transition_user_action = OFF +save +``` + +What this setup does: + +- `MIXER TRANSITION` starts one transition each time it moves from OFF to ON. +- Leaving `MIXER TRANSITION` ON does not restart the transition repeatedly. +- To request another transition, turn `MIXER TRANSITION` OFF, then ON again. +- If you turn `MIXER TRANSITION` OFF before the profile switch happens, INAV cancels that transition request. +- `mixer_vtol_transition_dynamic_mixer = OFF` keeps transition motor/servo behavior close to the older setup while the new controller only manages the timing of the profile switch. +- `mixer_switch_trans_timer = 50` means `5.0s`, because this setting is in deciseconds. + +Value direction notes: + +- Lower `mixer_switch_trans_timer`: timer-based transitions complete sooner, but may switch before the aircraft has enough speed. +- Higher `mixer_switch_trans_timer`: gives more time to accelerate or slow down, but keeps the aircraft in transition longer when pitot is not used. +- `mixer_vtol_transition_airspeed_timeout_ms = 0`: disables airspeed timeout aborts. Higher values wait longer before aborting an airspeed-controlled attempt; lower values abort sooner. +- `mixer_vtol_transition_scale_ramp_time_ms = 0`: no extra smooth motor/servo movement in this legacy-compatible baseline. Higher values are useful only when dynamic scaling is ON. +- `vtol_transition_to_fw_min_airspeed_cm_s = 0` and `vtol_transition_to_mc_max_airspeed_cm_s = 0`: use timer completion. Higher non-zero values enable pitot-based completion and make the speed condition stricter. +- `nav_vtol_mission_transition_user_action = OFF`: mission transition is disabled. Set it to `USER1`..`USER4` only after manual transition is validated. + +Typical 3-position switch layout: + +- Position 1: FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) +- Position 2: transition request (`MIXER PROFILE 2` ON, `MIXER TRANSITION` ON) +- Position 3: MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) + +Workflow: + +- Start in MC. +- Move to the transition position. +- INAV starts MC -> FW and changes profile after the configured condition is met. +- Move the switch to the FW position after the transition completes. +- Reverse the process for FW -> MC. + +In the standard layout above, the transition position deliberately has both `MIXER PROFILE 2` and `MIXER TRANSITION` ON. While the auto-transition controller is enabled, the controller owns the profile switching until `MIXER TRANSITION` is turned OFF again. + +Optional two-position desired-mode layout: + +```text +set vtol_autotransition_always = ON ``` -2. Set `mixer_switch_trans_timer` ds in cli in the MC mode mixer profile to specify the time required for your model to gain sufficient airspeed before transitioning to FW mode. +With this global option enabled, an armed manual switch from one VTOL mixer profile to the other does not switch profiles directly. Instead, it starts the auto-transition controller: + +- Switch to FW: INAV starts MC -> FW and ends in FW after the configured speed or timer condition. +- Switch to MC: INAV starts FW -> MC and ends in MC after the configured speed or timer condition. +- No middle transition position is required. +- If you move the switch back to the current profile before the profile switch happens, INAV aborts the transition and remains in the current profile. +- When disarmed, direct profile switching still works for bench and preflight checks. +- Turn this option OFF if you want direct VTOL MC/FW profile switching while armed. + +Leave `vtol_autotransition_always = OFF` if you want the pilot to keep the explicit choice between a direct profile switch and a separate `MIXER TRANSITION` request. + +Optional two-position layout with instant FW -> MC: + +Some pilots want an automated MC -> FW transition, but want FW -> MC to switch almost immediately when they request multicopter mode. This can still be done with `vtol_autotransition_always = ON` by making the FW -> MC transition complete by timer with a zero timer. + +Example: + +```text +set vtol_autotransition_always = ON +set vtol_transition_to_fw_min_airspeed_cm_s = 1300 +set vtol_transition_to_mc_max_airspeed_cm_s = 0 + +mixer_profile 1 +# FW profile: FW -> MC should switch immediately when MC is requested +set mixer_switch_trans_timer = 0 +set mixer_vtol_transition_scale_ramp_time_ms = 0 + +mixer_profile 2 +# MC profile: MC -> FW still waits for speed or the backup timer +set mixer_switch_trans_timer = 50 +set mixer_vtol_transition_scale_ramp_time_ms = 1200 ``` -mixer_profile 2or1 -set mixer_switch_trans_timer = 30 # 3s, 3000ms + +Behavior: + +- In MC, selecting FW starts MC -> FW and waits for `vtol_transition_to_fw_min_airspeed_cm_s`, or the MC profile's `mixer_switch_trans_timer` fallback if no usable airspeed source is available. +- In FW, selecting MC starts the controller, but `vtol_transition_to_mc_max_airspeed_cm_s = 0` chooses timer completion and the FW profile's `mixer_switch_trans_timer = 0` makes the profile switch happen immediately. +- `mixer_vtol_transition_scale_ramp_time_ms = 0` in the FW profile avoids extra smooth output movement after the immediate FW -> MC switch. Use a non-zero value only if you want outputs to continue moving smoothly after the switch. + +### Which mixer profile controls each direction + +Some transition settings are stored inside each mixer profile. For those settings, INAV uses the profile where the transition starts. Think of them as source-profile settings, not target-profile settings. + +With the example profile order used in this guide: + +| Direction | Starts from | Switches to | Put direction-specific mixer settings in | +| --- | --- | --- | --- | +| MC -> FW | `mixer_profile 2` / MC | `mixer_profile 1` / FW | `mixer_profile 2` | +| FW -> MC | `mixer_profile 1` / FW | `mixer_profile 2` / MC | `mixer_profile 1` | + +If your model uses the opposite profile order, keep the same rule: configure the transition in the profile where that transition starts. + +These settings are per-mixer-profile and follow the source-profile rule: + +- `mixer_vtol_manualswitch_autotransition_controller`: enables the manual auto-transition controller for transitions that start from that profile. +- `mixer_vtol_transition_dynamic_mixer`: enables smooth power/control/transition-servo movement for transitions that start from that profile. +- `mixer_switch_trans_timer`: backup timer for transitions that start from that profile. +- `mixer_vtol_transition_airspeed_timeout_ms`: airspeed wait limit for transitions that start from that profile. +- `mixer_vtol_transition_scale_ramp_time_ms`: time used for smooth output movement for transitions that start from that profile. +- `mixer_automated_switch`: allows navigation-owned profile changes that start from that profile, such as RTH MC -> FW, landing FW -> MC, and low-speed FW -> MC protection. Mission USER transitions use the mission transition settings and target profile checks, so they do not depend on this flag in the same way. + +Global transition settings are not stored per mixer profile. They apply by direction wherever that direction starts: + +- `vtol_transition_to_fw_min_airspeed_cm_s`: MC -> FW completion speed. This is only for MC -> FW. +- `vtol_transition_to_mc_max_airspeed_cm_s`: FW -> MC completion speed. This is only for FW -> MC. +- `vtol_fw_to_mc_auto_switch_airspeed_cm_s`: fixed-wing low-speed safety trigger. This can start FW -> MC from FW mode. +- `vtol_autotransition_always`: optional manual workflow. When armed, selecting the other VTOL profile starts the matching transition instead of directly switching profile. +- `vtol_transition_lift_min_percent`: lift motor power floor used by dynamic scaling in both directions. +- `vtol_transition_mc_authority_min_percent`: MC motor stabilisation floor used by dynamic scaling in both directions. +- `vtol_transition_fw_authority_min_percent`: FW control floor used by dynamic scaling in both directions. + +Servo mixer transition rules are profile rules too. Normal servo rules from the current active profile own the output while that profile is active, and normal servo rules from the target profile own it after the profile switch. The auto-transition controller can also use a narrow target-profile helper before the switch: if the target profile has a transition-linked rule for a servo output and the current profile does not, INAV can move that output smoothly from its current real position toward the target transition/static position. This is useful for tilt servos that are defined only in the MC profile during FW -> MC. + +Examples: + +- To make MC -> FW slow and speed-based, put the longer `mixer_switch_trans_timer`, non-zero `mixer_vtol_transition_scale_ramp_time_ms`, and MC -> FW dynamic mixer setup in the MC/source profile. +- To make FW -> MC switch immediately, put `mixer_switch_trans_timer = 0` and usually `mixer_vtol_transition_scale_ramp_time_ms = 0` in the FW/source profile, and set `vtol_transition_to_mc_max_airspeed_cm_s = 0`. +- To make FW -> MC move tilt servos before the MC profile becomes active, add the needed `INPUT_MIXER_TRANSITION`/`INPUT_MAX` tilt rules to the target MC profile. The controller can use those target rules during the transition even if the RC `MIXER TRANSITION` mode does not overlap the MC profile at that moment. + +### Timer, pitot, and timeout examples + +`mixer_switch_trans_timer` is the backup completion timer. It is used when no usable transition airspeed source is configured, trusted, or available. + +Examples: + +- `mixer_switch_trans_timer = 30`: transition completes by timer after `3.0s` if pitot is not used. +- `mixer_switch_trans_timer = 50`: transition completes by timer after `5.0s` if pitot is not used. +- `vtol_transition_to_fw_min_airspeed_cm_s = 1300`: MC -> FW waits for `13 m/s` airspeed when a usable transition airspeed source is available. +- `vtol_transition_to_mc_max_airspeed_cm_s = 850`: FW -> MC waits until airspeed falls to `8.5 m/s` or lower when a usable transition airspeed source is available. +- `mixer_vtol_transition_airspeed_timeout_ms = 6500`: if the airspeed source remains usable but the requested airspeed is not reached and confirmed within `6.5s`, that airspeed-controlled transition attempt is aborted. + +Important pitot behavior: + +- If a usable transition airspeed source is available and a non-zero airspeed threshold is configured, INAV prefers airspeed for transition completion. +- A usable transition airspeed source is either a valid real pitot sensor or `pitot_hardware = VIRTUAL` with a valid virtual airspeed estimate. +- The configured airspeed must be reached and remain on the safe side of the threshold for the internal `300ms` confirmation before `mixer_vtol_transition_airspeed_timeout_ms` expires. For example, with `vtol_transition_to_fw_min_airspeed_cm_s = 1300` and `mixer_vtol_transition_airspeed_timeout_ms = 6500`, MC -> FW must reach and confirm `13 m/s` within the total `6.5s` timeout. +- If that timeout expires during a manual transition, the transition attempt is aborted and INAV does not force the target profile switch from that timeout. +- If that timeout expires during a mission transition, mission retry/failure handling is used: retry can run if `nav_vtol_transition_retry_on_airspeed_timeout = ON`; otherwise the configured mission fail action is used. +- If the airspeed source becomes unavailable during the transition, INAV ignores a dropout shorter than the internal `300ms` confirmation window. If the source remains unavailable, INAV falls back to `mixer_switch_trans_timer`. +- With dynamic scaling enabled, a single high or low airspeed sample does not immediately change lift or stabilisation authority. INAV checks the direction of the airspeed-linked progress over an internal `300ms` confirmation window. Lift and MC/FW authority move forward only after a sustained increase in transition progress, and can move back together only after a sustained decrease. This allows real loss of airspeed to restore lift and MC authority during MC -> FW without reacting to short sensor steps. +- The same `300ms` airspeed window validates the final speed condition before the profile switch, including when dynamic scaling is OFF. There is no second independent airspeed confirmation timer. +- The airspeed confirmation does not delay the time-based forward-motor or tilt-servo movement. Those outputs still use `mixer_vtol_transition_scale_ramp_time_ms`, which avoids a tiltrotor waiting for airspeed before it starts tilting. +- If `pitot_hardware = VIRTUAL` is selected but virtual airspeed is not valid, for example no usable GPS/estimate, INAV falls back to `mixer_switch_trans_timer`. +- Raw ground speed is not used as an automatic pitot replacement for transition completion. If you want an estimated airspeed source, select `pitot_hardware = VIRTUAL`; then the Virtual Pitot logic owns the estimate. +- `mixer_vtol_transition_airspeed_timeout_ms` does not complete a transition. It only stops an airspeed-controlled attempt that is taking too long. + +Optional low-speed protection: + ``` -3. In your FW mode mixer profile (e.g., mixer_profile 1), also set `mixer_automated_switch` to `ON`. leave it to `OFF` if automated landing in fixed-wing is acceptable. +set vtol_fw_to_mc_auto_switch_airspeed_cm_s = 750 ``` -mixer_profile 1or2 + +With this set, fixed-wing flight automatically starts FW -> MC when the usable transition airspeed source drops to `7.5 m/s` or lower. Set it to `0` to disable this protection. + +This is a safety fallback for cases where fixed-wing flight no longer looks safe, for example a pusher problem or another failure that prevents the aircraft from keeping enough airspeed. + +It can be used in two contexts: + +- During manual FW flight, it requires `mixer_vtol_manualswitch_autotransition_controller = ON`. After the switch, INAV stays in MC until you deliberately command another manual profile change. +- During mission, RTH, and failsafe RTH, it is a navigation-owned safety transition. It requires `mixer_automated_switch = ON` and a valid MC target profile. After it switches to MC, INAV keeps the current navigation task in MC and does not automatically start another MC -> FW transition for that navigation session, even if a later mission waypoint requests FW. + +This setting decides when the emergency FW -> MC fallback starts. `vtol_transition_to_mc_max_airspeed_cm_s` still decides when the FW -> MC profile switch is safe to complete during an airspeed-controlled FW -> MC transition. + +## 2. Manual switch auto transition with dynamic scaling + +Dynamic scaling is the optional smooth part of the new transition system. It lets INAV change motor power and stabilisation strength gradually instead of making one large step at the profile switch. + +Enable it in both VTOL profiles: + +Per-mixer-profile settings: + +``` +mixer_profile 1 +set mixer_vtol_manualswitch_autotransition_controller = ON +set mixer_vtol_transition_dynamic_mixer = ON +set mixer_switch_trans_timer = 50 +set mixer_vtol_transition_airspeed_timeout_ms = 6500 +set mixer_vtol_transition_scale_ramp_time_ms = 1200 + +mixer_profile 2 +set mixer_vtol_manualswitch_autotransition_controller = ON +set mixer_vtol_transition_dynamic_mixer = ON +set mixer_switch_trans_timer = 50 +set mixer_vtol_transition_airspeed_timeout_ms = 6500 +set mixer_vtol_transition_scale_ramp_time_ms = 1200 +``` + +Global settings: + +``` +set vtol_transition_to_fw_min_airspeed_cm_s = 1300 +set vtol_transition_to_mc_max_airspeed_cm_s = 850 +set vtol_transition_lift_min_percent = 30 +set vtol_transition_mc_authority_min_percent = 20 +set vtol_transition_fw_authority_min_percent = 20 +save +``` + +What dynamic scaling changes: + +- MC -> FW can smoothly bring in the forward motor before the profile switch. +- MC -> FW can reduce lift motor power and MC motor stabilisation while fixed-wing control is increased. These three airspeed-linked changes use the same confirmed progress, so they stay synchronised and do not react independently to an airspeed spike. +- FW -> MC can remove the forward motor while lift motors and MC motor stabilisation come back. +- FW -> MC can reduce fixed-wing control as MC control comes back. +- Transition-linked servos continue from their current output if the profile switch would otherwise cause a step. + +What it does not change: + +- It does not decide by itself when the transition is complete. +- Completion still uses a usable transition airspeed source first, or `mixer_switch_trans_timer` when no usable source is available. +- With `mixer_vtol_transition_dynamic_mixer = OFF`, the old transition input behavior is preserved and the auto-controller only manages the profile switch timing. + +Value direction notes: + +- Lower `mixer_vtol_transition_scale_ramp_time_ms`: pusher, lift return, and tilt servo movement happen faster. Too low can still look abrupt. +- Higher `mixer_vtol_transition_scale_ramp_time_ms`: movement is gentler, but the aircraft spends longer with partial pusher/lift/servo authority. +- Lower `vtol_transition_lift_min_percent`: lift motors reduce more during MC -> FW. This can reduce drag/power use but gives less lift reserve. +- Higher `vtol_transition_lift_min_percent`: more lift is kept through transition. `100` keeps full lift power. +- Lower `vtol_transition_mc_authority_min_percent`: MC motor stabilisation is reduced more during MC -> FW. Use carefully on large VTOLs. +- Higher `vtol_transition_mc_authority_min_percent`: more MC stabilisation stays available. `100` keeps full MC stabilisation. +- Lower `vtol_transition_fw_authority_min_percent`: fixed-wing control starts more gently. Higher values bring fixed-wing control in more strongly from the start. +- Lower airspeed thresholds complete sooner; higher thresholds wait for more airspeed before switching profile. + +### `mixer_vtol_transition_scale_ramp_time_ms` + +This setting controls the time-based smooth movement for: + +- MC -> FW forward motor power increase +- FW -> MC forward motor power removal +- FW -> MC lift motor and MC motor stabilisation return +- `INPUT_MIXER_TRANSITION` movement when `mixer_vtol_transition_dynamic_mixer = ON` +- servo output continuation after a direct profile switch or transition abort changes the active mixer output + +Example: + +- `mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `mixer_switch_trans_timer = 50` + +Result: + +- The forward motor moves from idle to requested power over `1.2s` during MC -> FW. +- A tilt servo using `INPUT_MIXER_TRANSITION` moves over `1.2s` while dynamic scaling is ON. +- If the profile switch changes the final servo output, the servo continues from its current output and moves toward the new output over a fresh `1.2s`. +- If pitot is not used, the profile switch still happens after `5.0s`. +- If pitot is trusted, the profile switch waits for the configured pitot threshold instead of the `5.0s` timer. + +This separation is intentional. Tilt servos should not wait for airspeed to build before they start moving; otherwise the aircraft may need tilt to gain speed but also need speed before it is allowed to tilt. The tilt servo source uses `mixer_vtol_transition_scale_ramp_time_ms` when dynamic scaling is ON. + +### How the transition timers work together + +Several transition-related movements can happen during one transition. They are not all controlled by the same setting. + +`mixer_switch_trans_timer` + +- Unit: deciseconds. `50` means `5.0s`. +- Decides when a timer-based transition is allowed to switch to the target mixer profile. +- Used when there is no usable transition airspeed source, when the airspeed threshold for that direction is `0`, or when virtual airspeed is configured but not valid. +- Also gives the fallback progress used by airspeed-linked authority scaling when an airspeed source is not being used. +- Lower values switch profile sooner. Higher values keep the aircraft in the transition profile relationship longer. + +`mixer_vtol_transition_airspeed_timeout_ms` + +- Unit: milliseconds. +- Used only while INAV is actually waiting for a usable airspeed threshold. +- It does not complete a transition. +- If the threshold is not reached before this timeout, the transition attempt is aborted and manual or mission failure handling decides what happens next. +- If virtual pitot is configured but not valid, this timeout is not the normal completion timer; the transition is timer-based and uses `mixer_switch_trans_timer`. + +`mixer_vtol_transition_scale_ramp_time_ms` + +- Unit: milliseconds. +- Controls time-based smooth movement of power and transition-linked servo outputs. +- Does not decide when the profile switch happens. +- In MC -> FW it brings the forward motor in over this time. +- In MC -> FW it can move lift power down toward `vtol_transition_lift_min_percent` over the active transition progress. +- In FW -> MC it moves the forward motor down, and brings target MC lift power and MC motor stabilisation back over this time. +- In FW -> MC, if a lift motor exists only in the target MC profile, its real output starts from the current motor output and moves toward the target MC lift output over this time. +- `INPUT_MIXER_TRANSITION` uses this time when `mixer_vtol_transition_dynamic_mixer = ON`. +- After a profile switch or direct switch changes a transition-linked servo output, the servo continues from its current output toward the new output over a fresh window of this length. +- After a profile switch changes which motors exist in the active mixer, old propulsion outputs that must go to idle are moved toward idle over this time. + +Servo mixer `Speed` + +- This is the per-servo-rule speed column in the Servo Mixer table, in `10us/s`. +- It is separate from the VTOL transition timers. +- If set to `0`, that servo rule is not additionally slowed by the servo mixer speed limiter. +- If set to a non-zero value, it can slow that rule even if `mixer_vtol_transition_scale_ramp_time_ms` is shorter. + +Example with no usable airspeed source: + +- `mixer_switch_trans_timer = 50` +- `mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `mixer_vtol_transition_dynamic_mixer = ON` + +Result: + +- The target profile switch happens after about `5.0s`. +- Time-based power and transition-servo movement use about `1.2s`. +- In FW -> MC, target MC lift power can return much earlier than the final profile switch. +- If a target MC lift motor was idle in the FW profile, its real output starts from idle and moves toward the target MC lift output over the same `1.2s` instead of jumping immediately to the configured lift minimum. +- If the tilt servos are configured only in the MC profile, they cannot start moving during the FW-profile part of FW -> MC. They will only become active after the profile switch, because the FW profile does not own those servo rules. + +This timing relationship is important for tilt-rotor tricopter setups. If the tail rotor exists only in the MC profile, dynamic scaling can start applying target MC lift before the final MC profile switch. INAV therefore has a narrow safety helper for FW -> MC: when the target MC profile has `INPUT_MIXER_TRANSITION` rules for a servo output and the current FW profile does not, INAV can move that servo output from its current real position toward the target profile's transition/static position during the transition. This helper only uses `INPUT_MIXER_TRANSITION` and `INPUT_MAX` from the target profile; it does not apply target yaw, roll, pitch, or PID stabilisation before the MC profile is active. + +For target MC lift motors that exist only in the MC profile, INAV also captures the current real motor output and moves from that value toward the target MC lift output over `mixer_vtol_transition_scale_ramp_time_ms`. This prevents a tail/lift motor from jumping immediately from idle to `vtol_transition_lift_min_percent` at the start of FW -> MC. + +This means the auto-transition controller can use transition rules from the target mixer profile even when the RC `MIXER TRANSITION` mode is not active for that profile yet. The controller owns the transition while it is running. For example, a normal 3-position switch may have `MIXER TRANSITION` active only in the middle switch position while the aircraft is still using the FW profile. If the target MC profile contains tilt-servo `INPUT_MIXER_TRANSITION` rules, INAV can still use those target rules to move the tilt servos smoothly toward the MC transition position before the MC profile is finally selected. + +### Percentage settings + +These settings are active only when `mixer_vtol_transition_dynamic_mixer = ON`. + +`vtol_transition_lift_min_percent` + +- Current purpose: sets the lowest lift motor base throttle used during transition. +- This is the throttle/lift part of the MC lift motor output, not the roll/pitch/yaw stabilisation correction. +- MC -> FW: lift motor base throttle is reduced toward this value. +- FW -> MC: target MC lift motor base throttle starts from this value and returns to full power. +- `100` keeps full lift base throttle for the whole transition. +- This is one shared value for both directions. A high value keeps lift during MC -> FW, but it also makes FW -> MC start with stronger target MC lift. On tricopter or tail-rotor VTOLs this can make the tail rotor active early while the aircraft is still in FW geometry. +- If a lift motor exists only in the target MC profile, the real motor output is still smoothed from its current value toward this target lift output when dynamic scaling and `mixer_vtol_transition_scale_ramp_time_ms` are enabled. + +Example: + +- `vtol_transition_lift_min_percent = 30` +- At halfway through the lift-power change, lift power is about `65%`. +- At the lowest point, lift power is `30%`, not zero. + +For timer-based FW -> MC tilt-rotor tuning: + +- Lower values reduce early tail/lift motor power, but give less tail support during the transition. +- Higher values keep more lift support, but can push the tail strongly before the front motors are tilted for MC flight. +- `100` is useful if you need full lift retained during MC -> FW, but it also means FW -> MC starts from full target lift. If you need full lift in MC -> FW but a gentle tail rotor start in FW -> MC, this single setting may not be enough and the setup may need a direction-specific lift-start option in a future firmware change. + +`vtol_transition_mc_authority_min_percent` + +- Current purpose: sets the lowest MC roll/pitch/yaw stabilisation correction during transition. +- This is the stabilisation correction mixed into MC motors, not the lift motor base throttle. +- MC -> FW: active MC motor stabilisation correction is reduced toward this value. +- FW -> MC: target MC motor stabilisation correction starts from this value and returns to full strength. +- During FW -> MC, this target MC stabilisation correction comes from the MC mixer profile and target MC PID preview, not from the active FW controller. + +Example: + +- `vtol_transition_mc_authority_min_percent = 20` +- MC roll/pitch/yaw correction never goes below `20%` during the transition. + +`vtol_transition_fw_authority_min_percent` + +- Current purpose: sets the lowest fixed-wing stabilisation strength during transition. +- MC -> FW: fixed-wing control starts from this value and increases to full strength. +- FW -> MC: fixed-wing control is reduced toward this value. +- When `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules are configured in the MC profile, this setting also controls how strongly those target fixed-wing servo corrections are applied during MC -> FW. + +Example: + +- `vtol_transition_fw_authority_min_percent = 20` +- Fixed-wing control starts gently, then increases as the transition progresses. +- `100` means fixed-wing control is full strength for the whole transition. + +### Manual throttle during transition + +The smooth transition controller does not automatically reinterpret the pilot throttle stick as hover throttle in all modes. It scales the motor outputs that the current control mode is already producing. + +During FW -> MC, `nav_mc_althold_throttle = STICK` can be helpful when ALTHOLD/NAV is engaged after the transition. It lets INAV treat the current throttle stick position as the neutral altitude-hold point, so the aircraft is less likely to jump up or sink when MC altitude hold takes over. + +During MC -> FW, the pilot throttle position still matters. If the aircraft enters transition with very low MC throttle, it may lose altitude before the wing and forward motor are carrying the aircraft. If it enters transition while the pilot is commanding a strong vertical climb, the lift motors may keep trying to climb while the aircraft is also accelerating forward. Dynamic scaling can reduce this effect by reducing lift motor base throttle toward `vtol_transition_lift_min_percent`, but it does not replace careful throttle management. + +When MC altitude hold is active, `nav_mc_manual_climb_rate` still limits the climb/descent rate requested by pilot throttle input. This can help prevent an accidental large manual climb command from carrying too much vertical lift into MC -> FW transition. In pure manual throttle modes, use pilot technique and transition tuning instead: start MC -> FW near hover throttle, avoid commanding a strong vertical climb into transition, tune `vtol_transition_lift_min_percent`, and choose a `mixer_vtol_transition_scale_ramp_time_ms` that gives the aircraft enough time to transfer lift from the rotors to the wing. + +### Pusher configuration example + +For a pusher VTOL, the preferred smooth setup is: + +- In the FW profile, configure the pusher as a normal positive-throttle motor. +- In the MC profile, reserve the same motor index with a placeholder rule if that motor is not used in MC flight. +- Use `throttle = -1.000`, `roll = 0`, `pitch = 0`, `yaw = 0` for the placeholder if Configurator removes zero-throttle motor rules. + +Example shape: + +``` +# MC profile: reserve motor 5 as a placeholder +mixer_profile 2 +mmix 4 -1.000 0.000 0.000 0.000 + +# FW profile: motor 5 is the real forward motor +mixer_profile 1 +mmix 4 1.000 0.000 0.000 0.000 +``` + +What happens with dynamic scaling ON: + +- MC -> FW: motor 5 starts from idle and smoothly reaches the requested FW throttle over `mixer_vtol_transition_scale_ramp_time_ms`. +- FW -> MC: motor 5 smoothly moves back to idle over `mixer_vtol_transition_scale_ramp_time_ms`. +- If motor 5 is not used by the destination profile after the switch, INAV keeps moving it toward idle instead of stopping it in one step. + +The older helper rule still works: + +``` +mmix 4 -1.200 0.000 0.000 0.000 +``` + +That older style spins the pusher at a fixed helper power only while `MIXER TRANSITION` is active. It can still be useful for legacy setups, but it is not the recommended setup for the smooth auto-transition controller. + +### Tilt servo configuration example + +Tilt servos commonly use: + +- `INPUT_MAX` (`29`) as a fixed offset +- `INPUT_MIXER_TRANSITION` (`38`) as the transition movement +- `INPUT_STABILIZED_YAW` (`2`) for tricopter/tiltmotor yaw correction + +Example based on a tricopter/tiltmotor style setup: + +``` +mixer_profile 2 +set platform_type = TRICOPTER + +smix reset +smix 0 4 2 -50 0 -1 # yaw correction on servo 4 +smix 1 5 2 -50 0 -1 # yaw correction on servo 5 +smix 2 4 29 87 0 -1 # fixed tilt offset on servo 4 +smix 3 5 29 -87 0 -1 # fixed tilt offset on servo 5 +smix 4 4 38 -45 0 -1 # transition tilt movement on servo 4 +smix 5 5 38 45 0 -1 # transition tilt movement on servo 5 +``` + +Behavior: + +- `INPUT_MAX` stays constant. +- With dynamic scaling OFF, `INPUT_MIXER_TRANSITION` keeps the older fixed transition value while transition mode is active. +- With dynamic scaling ON, `INPUT_MIXER_TRANSITION` moves from `0` to `500` over `mixer_vtol_transition_scale_ramp_time_ms`. +- During FW -> MC, the value moves back from `500` toward `0` over `mixer_vtol_transition_scale_ramp_time_ms`. +- During MC -> FW, `INPUT_MIXER_TRANSITION` must not move backwards when the FW profile is selected. +- During FW -> MC, it must also continue smoothly if the transition is aborted or reversed. +- If the destination profile does not own the same tilt servo output, INAV keeps the current servo output and moves it toward the destination output instead of briefly jumping through a middle/default position. + +If the front tilt servos use `INPUT_MIXER_TRANSITION` only in the MC profile, INAV can preview only the static transition movement during FW -> MC. This is useful when the FW profile has no tilt-servo rules, but the MC profile defines the physical tilt positions. The preview is intentionally limited: INAV uses the target profile's `INPUT_MIXER_TRANSITION` rule and any `INPUT_MAX` fixed offset for that same servo output. It does not apply target yaw correction, roll/pitch correction, or other stabilised corrections until the MC profile is active. + +`INPUT_MIXER_TRANSITION + INPUT_MAX` in this context means INAV adds the target transition movement and the target fixed offset if both rules exist. A servo can have both rules, or only the transition rule. A `MAX` rule alone is not enough to identify a transition servo, because many normal servos use fixed offsets. + +For tilt-rotor aircraft with a tail lift motor, this matters because target MC lift can be introduced before the final MC profile switch when dynamic scaling is ON. If the front motors are still held in FW tilt position, too much early tail motor power can push the tail up and the nose down. Typical tuning options are: + +- Increase `mixer_vtol_transition_scale_ramp_time_ms` so target MC lift returns more slowly. +- Lower `vtol_transition_lift_min_percent` so FW -> MC starts with less target lift, then test carefully for tail drop. +- Keep `mixer_switch_trans_timer` long enough for the aircraft to slow down and for the mechanical tilt movement to be in a safe range before the profile switch. + +Example: + +- MC tilt position: `90 degrees` +- transition tilt position from source 38: `45 degrees` +- FW profile final position: `0 degrees` +- `mixer_vtol_transition_scale_ramp_time_ms = 1200` + +Expected movement: + +- The servo moves from `90` toward `45` over about `1.2s`. +- If the profile switch then changes the final target from `45` to `0`, the servo continues from its current output and moves toward `0` over a fresh `1.2s`. + +### Optional fixed-wing control preview on MC profile + +If your aircraft has control surfaces that should start helping before the profile switch, add `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules in the MC mixer profile. + +Useful inputs: + +- `INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL` (`61`) +- `INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH` (`62`) +- `INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW` (`63`) +- positive-only and negative-only variants `64..69` + +During MC -> FW, these rules let the MC profile apply a preview of the target fixed-wing stabilisation. The preview uses the target fixed-wing PID bank, rates, angle limits, heading-hold limits, and turn-assist gains. During FW -> MC, the same MC profile rules mark which FW servo outputs should reduce their fixed-wing correction while MC motor control comes back. + +If you do not configure these rules, pusher and tilt transition can still work. This preview is optional and mainly helps aircraft where control surfaces should start contributing before the final profile switch. + +## 3. Automated mission transition (fully autonomous flight) + +Mission transition lets a waypoint request MC or FW mode. The mission pauses while INAV performs the transition, then resumes after the transition completes. + +### Required setup + +Enable navigation-requested profile changes in profiles where NAV is allowed to change mode: + +``` +mixer_profile 1 set mixer_automated_switch = ON + +mixer_profile 2 +set mixer_automated_switch = ON +save +``` + +Enable the waypoint USER flag that will select the VTOL mode: + ``` -4. Save your settings. type `save` in cli. +set nav_vtol_mission_transition_user_action = USER1 +set nav_vtol_mission_transition_min_altitude_cm = 1200 +set nav_vtol_transition_retry_on_airspeed_timeout = ON +set nav_vtol_transition_fail_action_mc_to_fw = POSH +set nav_vtol_transition_fail_action_fw_to_mc = LOITER +save +``` + +How waypoint USER selection works: + +- selected USER bit clear (`0`) means target MC profile +- selected USER bit set (`1`) means target FW profile +- every navigable waypoint should intentionally set or clear that USER bit +- if the aircraft is already in the requested profile, INAV continues without starting a transition + +Example mission: + +- WP1: USER1 not set, climb in MC near the takeoff area +- WP2: USER1 set, request MC -> FW transition +- WP3: USER1 set, continue fixed-wing navigation +- LAND or final approach waypoint: USER1 not set, request FW -> MC before landing + +For MC -> FW mission transition, INAV uses a straight acceleration segment. It does not try to loiter to build airspeed. Normal waypoint advancement is paused until the transition is finished. + +For either direction, a successful transition requested by an ordinary `WAYPOINT` finishes that waypoint and proceeds directly to the next one. This avoids an unwanted circle after a transition-induced climb: for example, if an MC -> FW transition starts at `120m` and forward thrust briefly carries the aircraft to `127m`, INAV does not make a fixed-wing aircraft loiter back down to `120m` before continuing. + +- This shortcut is used only after the transition has completed normally, including its output handover period. +- The aircraft must still not be below the waypoint altitude minus the `nav_wp_enforce_altitude` tolerance. A transition that finishes too low continues using the normal altitude hold behavior. +- It applies to ordinary `WAYPOINT` actions in both MC -> FW and FW -> MC directions. +- `HOLD_TIME` and `LAND` actions always keep their requested behavior. Abort, an ultimately failed retry, and forced-profile fallback also do not mark the waypoint complete. + +### Altitude settings that matter in missions + +`nav_vtol_mission_transition_min_altitude_cm` + +- Current purpose: minimum altitude before a mission-requested VTOL transition may start. +- New transition use: prevents a mission from starting MC -> FW too low. +- `0` disables this gate. + +Example: + +- `nav_vtol_mission_transition_min_altitude_cm = 1200` +- Mission MC -> FW transition waits until current altitude is at least `12m` above the navigation reference. + +`nav_wp_enforce_altitude` + +- Existing purpose: controls whether a waypoint is considered reached only by horizontal distance, or also by altitude. +- New VTOL mission use: helps prevent the aircraft from accepting a transition waypoint horizontally while still far below the requested waypoint altitude. + +Examples: + +- `nav_wp_enforce_altitude = 0`: waypoint can be accepted by horizontal radius even if altitude is still not reached. This is faster, but can be risky for VTOL transition missions. +- Example with `nav_wp_enforce_altitude = 0`: if WP1 is directly above home at `120m`, the aircraft can be inside the horizontal waypoint radius immediately after takeoff. The waypoint may be accepted before the aircraft climbs to `120m`, so the mission can continue to the next action too early. +- `nav_wp_enforce_altitude = 100`: waypoint must be within about `1m` of the target altitude before it is considered reached. +- `nav_wp_enforce_altitude = 300`: waypoint must be within about `3m`; this is more relaxed and can be useful when altitude estimates are noisy. + +`nav_wp_radius` + +- Existing purpose: horizontal radius used to decide whether a normal waypoint has been reached. +- VTOL mission impact: if this is very large, a waypoint can be accepted early. Use `nav_wp_enforce_altitude` and `nav_vtol_mission_transition_min_altitude_cm` to avoid starting a transition before the aircraft is high enough. +- Landing safety use: VTOL MC landing settle uses a stricter internal landing capture radius based on `min(nav_wp_radius, 100cm)`. + +Examples: + +- `nav_wp_radius = 200`: waypoint accepted within `2m` horizontally. +- `nav_wp_radius = 1000`: waypoint accepted within `10m`; useful for fast FW navigation, but too loose by itself for deciding that a VTOL has settled at a landing point. + +### Mission transition failure behavior + +If transition cannot start because of temporary runtime conditions, INAV waits instead of immediately failing the mission. Temporary conditions include sensor calibration still in progress, navigation position temporarily unusable, heading temporarily unusable, or another transition already active. + +Hard safety/configuration problems are treated as errors or use the configured fail action where applicable. Examples include disarmed state, failsafe, missing mixer-profile mode setup, or no valid profile switch path. + +MC -> FW fail action: + +- `IDLE`: stop mission navigation and leave the pilot to recover +- `POSH`: enter position hold if possible +- `RTH`: return to home +- `EMERGENCY_LANDING`: start emergency landing behavior + +FW -> MC fail action: + +- `IDLE`: stop mission navigation and leave the pilot to recover +- `LOITER`: loiter in fixed-wing if possible +- `RTH`: return to home +- `EMERGENCY_LANDING`: start emergency landing behavior +- `FORCE_SWITCH`: force the target mixer switch even though the normal transition condition failed + +Use `FORCE_SWITCH` only when you have tested the airframe carefully. It is intended as a last-resort choice for setups where staying in FW is more dangerous than switching. + +`nav_vtol_transition_retry_on_airspeed_timeout = ON` allows one retry after an airspeed timeout. If the retry also fails, the configured fail action is used. + +## 4. Adding VTOL stabilisation + +A VTOL in MC mode is not always equivalent to a normal multicopter. Large wings can still create lift and drag while the aircraft is braking, yawing, descending, or correcting altitude. That can turn normal MC commands into unexpected climb, sink, roll, pitch, or yaw coupling. + +The VTOL MC protection feature is disabled by default: + +``` +set vtol_mc_protection_mode = OFF +``` + +This means: + +- normal multicopters do not change +- fixed-wing mode does not change +- existing VTOL behavior does not change unless you enable the feature + +### Protection modes + +`vtol_mc_protection_mode = OFF` + +- Current behavior: no new VTOL MC protection. +- Use this when comparing against older behavior. + +`vtol_mc_protection_mode = NAV` + +- Adds VTOL MC protections during navigation and automatic-throttle operation. +- Applies only when INAV detects an active multicopter-like profile and another configured profile is fixed-wing-like. +- Does not affect fixed-wing flight. +- Does not affect normal non-VTOL multicopters. -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. +With `NAV`, INAV can: +- reserve throttle range for attitude stabilisation before altitude controller anti-windup limits are applied +- keep hover throttle inside the safe range, shrinking the reserve if necessary +- let the existing MC position controller reduce horizontal speed before fully locking the final position-hold point +- relax altitude target capture while the aircraft is bleeding speed or transitioning in MC +- require a more stable landing condition before starting/describing landing as complete +- use a conservative recovery path if attitude becomes excessive during automatic-throttle VTOL MC navigation + +`vtol_mc_protection_mode = NAV_AND_STABILIZED` + +- Includes everything from `NAV`. +- Also shapes pilot roll, pitch, and yaw commands in ANGLE and HORIZON mode when the aircraft is armed, in VTOL MC mode, not fixed-wing, velocity estimate is trusted, and horizontal speed is high enough. +- This is the mode to use when large wings make manual ANGLE/HORIZON MC flight feel too aggressive at forward speed. + +### Roll, pitch, and yaw command shaping + +The amount of shaping changes continuously with horizontal speed: + +- below about `300 cm/s`, commands are unchanged +- between about `300 cm/s` and `800 cm/s`, commands are gradually reduced +- above about `800 cm/s`, commands are limited to about 50% of the original command + +Example: + +- At `250 cm/s`, yaw/roll/pitch stick response is normal. +- At `550 cm/s`, the same stick movement commands a smaller rate/angle than usual. +- At `900 cm/s`, the same stick movement commands about half of the normal response. + +This is intended to reduce wing-driven surprises during fast MC-mode flight, not to make the aircraft unresponsive. If velocity estimate is not trusted, this shaping is not applied. + +### Throttle reserve for attitude authority + +`vtol_mc_thr_reserve_percent` + +- Current purpose: reserves throttle range so altitude control cannot consume all motor authority. +- Applies when VTOL MC protection is active. +- The reserve is applied before altitude controller anti-windup bounds, not only as a final output clamp. +- Hover throttle is kept inside the safe range. If the configured reserve would exclude hover throttle, INAV shrinks the reserve and sets a debug flag. + +Example: + +- `nav_mc_hover_thr = 1500` +- `vtol_mc_thr_reserve_percent = 15` +- altitude control is not allowed to command all the way to minimum throttle, because very low base throttle can leave roll, pitch, and yaw corrections with little effective motor authority +- altitude control is not allowed to command all the way to maximum throttle, because saturated motors have no upward headroom left for attitude correction +- roll, pitch, and yaw still have headroom for correction + +If `vtol_mc_thr_reserve_percent = 0`, the throttle range is not narrowed, but the other enabled VTOL MC protections can still run. + +### Altitude tolerance while braking or transitioning + +During VTOL MC braking, wing lift can make the aircraft climb even when the pilot or NAV only intended to slow down. If altitude hold reacts too aggressively, it can reduce throttle too far, then later command too much throttle, leaving less room for attitude stabilisation. + +With VTOL MC protection enabled, INAV is more tolerant while the aircraft is still settling: + +- NAV capture lets the existing MC position controller reduce velocity first instead of immediately forcing a hard final hold point. +- Soft altitude capture follows current altitude more gently while horizontal speed is being reduced. +- During auto transition while the active side is MC, the same protection can reduce altitude-control aggression. +- Once the aircraft is stable, normal altitude/position behavior resumes. + +This is meant to cover the case where the wings affect MC behavior during braking, yawing, and transition. + +## 5. Landing detection setup + +Landing detection is safety-critical for VTOL. A large VTOL wing can make the aircraft bounce in ground effect, and barometric altitude alone cannot prove that the aircraft is on the ground. + +The new VTOL MC landing logic keeps the normal INAV landing detector, but adds extra checks before VTOL MC can report `LANDING_DETECTED`. + +### Basic landing settings + +`nav_rth_allow_landing` + +- Existing purpose: controls whether RTH is allowed to land. +- Values are `NEVER`, `ALWAYS`, and `FS_ONLY`. +- If landing is not allowed, RTH will not intentionally descend to land. + +`nav_disarm_on_landing` + +- Existing purpose: allows automatic disarm after landing detection. +- If OFF, landing may be detected but the FC will not automatically disarm because of landing. + +`nav_auto_disarm_delay` + +- Existing purpose: delay after landing detection before automatic disarm. +- VTOL MC use: delay still applies, but VTOL MC must first pass its additional landing confirmation checks. + +`nav_land_detect_sensitivity` + +- Existing purpose: scales the generic landing detector velocity and gyro thresholds. +- At default `5`, MC landing detection uses about `100 cm/s` horizontal speed, `100 cm/s` vertical speed, and `4 deg/s` average pitch/roll gyro threshold. +- Higher values make detection easier and earlier, but increase false-detect risk. +- VTOL MC use: this setting can create a landing candidate, but it cannot bypass VTOL MC vertical-speed and throttle-probe confirmation. + +Examples: + +- `nav_land_detect_sensitivity = 5`: conservative default. +- `nav_land_detect_sensitivity = 7`: detects more easily, useful if the craft bounces on touchdown, but test carefully. +- `nav_land_detect_sensitivity = 10`: much more relaxed; not recommended for initial VTOL testing because it can create landing candidates while still airborne. + +`nav_landing_bump_detection` + +- Existing purpose: allows a touchdown acceleration bump to become a landing candidate. +- VTOL MC use: a bump is not an immediate disarm shortcut. Trusted high AGL blocks it, and accepted candidates must still pass throttle-probe confirmation. + +### Descent speed and final landing behavior + +`nav_land_maxalt_vspd` + +- Existing purpose: requested vertical descent speed above `nav_land_slowdown_maxalt` during RTH landing. + +`nav_land_minalt_vspd` + +- Existing purpose: requested vertical descent speed under `nav_land_slowdown_minalt` during RTH landing. +- VTOL MC use: also provides the conservative vertical-speed reference for final landing settle checks, capped internally so it cannot become too loose. +- This is used because it represents the descent speed you already consider acceptable near the ground. + +`nav_land_slowdown_maxalt` + +- Existing purpose: altitude where RTH landing starts slowing from `nav_land_maxalt_vspd` toward `nav_land_minalt_vspd`. + +`nav_land_slowdown_minalt` + +- Existing purpose: altitude where RTH landing should already be using `nav_land_minalt_vspd`. + +Examples: + +- `nav_land_minalt_vspd = 50`: final descent target is `0.5 m/s`; VTOL final landing checks stay conservative. +- `nav_land_minalt_vspd = 100`: final descent target is `1.0 m/s`; VTOL final landing checks allow a faster near-ground descent, but remain internally capped. +- `nav_land_maxalt_vspd = 200`: descent above the slowdown window can be `2.0 m/s`. +- `nav_land_slowdown_maxalt = 2000` and `nav_land_slowdown_minalt = 500`: descent slows between `20m` and `5m` above the landing reference. + +### VTOL MC throttle-probe confirmation + +VTOL MC landing candidates must pass a short lift-throttle confirmation. + +In plain language: + +- INAV sees a possible landing. +- Instead of immediately reporting landed, it gently reduces lift throttle for a short confirmation window. +- If the aircraft starts falling away, AGL drops, low-G/unloading is detected, or vertical speed changes toward a stronger descent, the landing candidate is rejected. +- If that small throttle reduction does not produce airborne-response evidence during the confirmation window, landing can be reported. + +This avoids a common false positive: the aircraft is still high in the air, vertical speed is temporarily low, and gyro rates are calm, so the generic detector thinks it might be landed. A real airborne VTOL should react to a small lift-throttle reduction; a landed aircraft should not fall away like an airborne one. + +The confirmation is not meant to reject every bounce, rocking motion, or pitch/roll wobble by itself. A VTOL can bounce in ground effect shortly before real touchdown. The important question for this check is whether reducing lift throttle causes the aircraft to continue descending like it is still flying. If it does, INAV rejects the candidate and waits for another landing opportunity. + +The confirmation does not rely on barometric altitude as proof of AGL. If a trusted surface/AGL sensor is available, it is used as an additional safety input. Without trusted AGL, vertical motion and acceleration are more important than baro altitude. + +`nav_mc_hover_thr` matters here, but the probe does not assume that landing throttle is equal to hover throttle. It starts from the current adjusted throttle at the moment of the landing candidate, which during descent may already be below `nav_mc_hover_thr`. `nav_mc_hover_thr` is used to size a small bounded throttle reduction relative to idle/hover range. If the current landing throttle is already below that probe limit, INAV does not raise it just to run the probe. Tune hover throttle before relying on automatic VTOL landing because it still affects throttle reserve and the probe reduction size. + +### Landing settle before descent + +VTOL MC landing should not start descent just because the aircraft briefly touches a large waypoint radius. + +For VTOL MC landing settle: + +- INAV uses a tighter landing capture radius based on `min(nav_wp_radius, 100cm)`. +- Horizontal speed must be low. +- Vertical speed must be low when a vertical estimate is available. +- Roll/pitch attitude must be within a safe range. +- Conditions must stay stable long enough before landing descent/landing detection is allowed to proceed. + +Examples: + +- `nav_wp_radius = 500`: normal waypoint acceptance is `5m`, but VTOL landing settle uses `1m`. +- `nav_wp_radius = 80`: VTOL landing settle uses `80cm`. +- `nav_wp_radius = 1500`: normal waypoint acceptance is `15m`, but VTOL landing settle still uses `1m`. + +This lets fixed-wing missions keep a larger waypoint radius while preventing VTOL landing from starting just because the aircraft briefly crossed a loose radius. + +### Practical landing setup + +Conservative starting point: + +``` +set nav_rth_allow_landing = ALWAYS +set nav_disarm_on_landing = ON +set nav_auto_disarm_delay = 1000 +set nav_land_detect_sensitivity = 5 +set nav_landing_bump_detection = ON +set nav_land_minalt_vspd = 50 +set nav_land_maxalt_vspd = 150 +set nav_land_slowdown_maxalt = 2000 +set nav_land_slowdown_minalt = 500 +set vtol_mc_protection_mode = NAV +save +``` + +If the aircraft bounces on landing: + +- Do not immediately raise `nav_land_detect_sensitivity` a lot. +- First reduce final descent energy: lower `nav_land_minalt_vspd` if possible, tune hover throttle, and verify `nav_mc_hover_thr`. +- Keep `nav_landing_bump_detection = ON` so a real touchdown bump can help create a candidate. +- Increase `nav_land_detect_sensitivity` only in small steps, and verify with blackbox logs. + +## Debugging and setting scope + +Useful debug modes: + +- `set debug_mode = VTOL_TRANSITION`: transition phase, transition direction, progress, motor scaling, servo transition progress, and profile-switch smoothing state. +- `set debug_mode = VTOL_MC_PROTECT`: VTOL MC protection flags, safe throttle min/max, protected throttle, speed, attitude, and command-shaping/settle progress. +- `set debug_mode = LANDING`: normal landing detector path and landing candidate state. + +`VTOL_TRANSITION` debug channels: + +- `debug[0]`: transition phase (`0=IDLE`, `1=TRANSITION_INITIALIZE`, `2=TRANSITIONING`, `3=after-switch smoothing`). +- `debug[1]`: active transition request, with direction packed in bits `8..15` and wait reason packed in bits `16..23`. + - wait reason `0`: none. + - wait reason `1`: FW -> MC is close to the MC speed threshold, but is still waiting. + - wait reason `2`: no usable transition airspeed source is available, so timer-based completion is being used. + - wait reason `3`: FW -> MC is waiting because the usable airspeed source is still clearly above `vtol_transition_to_mc_max_airspeed_cm_s`. +- `debug[2]`: packed flags. Important bits include direction, controller active, transition input active, airspeed path active, profile switch done, abort state, current/next profile index, mission active, failsafe active, direct profile switch active, target preview mode, after-switch smoothing active, and legacy manual session active. +- `debug[3]`: main transition progress x1000 (`0..1000`). +- `debug[4]`: pusher/forward motor scale x1000 (`0..1000`). +- `debug[5]`: lift motor scale x1000 (`0..1000`). +- `debug[6]`: while transition is active, packed MC/FW stabilisation scales. Low 16 bits are MC stabilisation scale x1000, high 16 bits are FW control scale x1000. When the transition controller is idle, this becomes servo diagnostic slot 0. +- `debug[7]`: while transition is active, packed progress values. Bits `0..9` are airspeed-linked scaling progress, bits `10..19` are motor ramp progress, and bits `20..29` are after-switch smoothing progress. When the transition controller is idle, this becomes servo diagnostic slot 1. + +When dynamic scaling and airspeed are used, `debug[3]` is the immediate raw progress and the low 10 bits of `debug[7]` are the confirmed progress actually used for airspeed-linked scaling. A short pitot step can therefore appear in `debug[3]` without moving lift or authority in `debug[5]`/`debug[6]`. A real sustained airspeed decrease can move the confirmed progress back after the confirmation window. + +Idle servo diagnostic slots are intended for bench/debug logs after a transition has finished. They help determine whether a servo output is already wrong in the servo mixer, is changed by transition preview/smoothing, or is changed later by the output/filter path. + +Decode `debug[6]` or `debug[7]` in idle state as: + +- servo index: `debug & 0x0f`. +- servo output before transition preview/smoothing: `((debug >> 4) & 0x7ff) + 900` microseconds. +- servo output after transition preview/smoothing: `((debug >> 15) & 0x7ff) + 900` microseconds. +- flags: `(debug >> 26) & 0x3f`. +- flag bit `0`: transition preview/smoothing changed this servo output. +- flag bit `1`: after-switch servo smoothing still owns this servo. +- flag bit `2`: target-profile transition preview captured this servo. + +`VTOL_MC_PROTECT` debug channels: + +- `debug[0]`: flags bitmask. Bits show protection configured, VTOL MC detected, NAV protection active, ANGLE/HORIZON protection active, NAV capture active, landing settle active, bailout active, throttle reserve shrunk, soft altitude capture active, roll/pitch/yaw command shaped, and velocity fallback used. +- `debug[1]`: safe throttle minimum. +- `debug[2]`: safe throttle maximum. +- `debug[3]`: protected throttle. +- `debug[4]`: horizontal speed [cm/s]. +- `debug[5]`: vertical speed [cm/s]. +- `debug[6]`: max absolute roll/pitch attitude [deci-degrees]. +- `debug[7]`: capture/landing/bailout settle elapsed time [ms], or command scale x1000 when command shaping is active, otherwise `1000`. + +OSD system messages: + +INAV shows VTOL transition progress in the normal OSD system message field, including analog/MSP DisplayPort OSD and DJI HD OSD message output. + +Typical messages include: + +- `VTOL MANUAL TO FW` or `VTOL MANUAL TO MC`: manual switch transition is running. +- `VTOL MISSION TO FW` or `VTOL MISSION TO MC`: waypoint USER action requested a mission transition. +- `VTOL RTH TO FW`: RTH requested MC -> FW before flying home. +- `VTOL LAND TO MC`: RTH or mission landing requested FW -> MC before MC landing. +- `VTOL LOW SPD TO MC`: low-speed protection requested FW -> MC as a safety fallback. +- `WAIT MC SPEED`: FW -> MC is close to the MC speed threshold, but is still waiting. +- `NO SPEED`: no usable transition airspeed source is available for this transition, so INAV is using the timer path. +- `MC SPEED HIGH`: FW -> MC is waiting because the usable airspeed source is still clearly above `vtol_transition_to_mc_max_airspeed_cm_s`. + +For display only, `MC SPEED HIGH` means the usable airspeed value is more than about `1 m/s` above the MC switch threshold. Inside that final `1 m/s` band, INAV shows `WAIT MC SPEED` until the actual threshold is reached. The real profile-switch condition remains `airspeed <= vtol_transition_to_mc_max_airspeed_cm_s`. +- `VTOL RETRY SCAN`: mission/RTH MC -> FW timed out on airspeed and INAV is scanning headings for a better airspeed direction. +- `VTOL RETRY ALIGN`: INAV is turning to the best heading found by the retry scan before trying again. +- `VTOL FINISHING SWITCH`: the target profile is already active and old propulsion output is being moved to its safe final value. +- `VTOL AIRSPEED TIMEOUT`: the requested pitot airspeed was not reached before `mixer_vtol_transition_airspeed_timeout_ms`. +- `VTOL TRANSITION ABORTED`: the transition was cancelled before the profile switch. +- `VTOL ABORTED IN FW`: the transition was cancelled and the active profile remains fixed-wing. +- `VTOL ABORTED IN MC`: the transition was cancelled and the active profile remains multicopter. +- `VTOL TRANSITION DONE`: the transition finished. + +Per-mixer-profile settings: + +These values are read from the source mixer profile, meaning the profile where the transition starts. In the standard setup in this document, MC -> FW uses the MC profile values and FW -> MC uses the FW profile values. + +- `mixer_automated_switch`: existing RTH use is to allow NAV to switch from MC to FW for return and from FW to MC for landing. New safety use is to allow navigation-owned low-speed FW -> MC protection. This flag is checked in the source profile for those navigation-owned transitions. Mission USER transitions are controlled by the mission VTOL settings and target profile checks instead. +- `mixer_switch_trans_timer`: existing legacy transition timer in deciseconds. New auto-transition use is the fallback completion timer for transitions starting from this profile when pitot airspeed is not used or not trusted. +- `mixer_vtol_transition_dynamic_mixer`: new optional smooth power/control scaling for transitions starting from this profile. OFF keeps old transition motor/servo behavior; ON allows smooth pusher, lift, MC stabilisation, FW control, and transition-servo movement. +- `mixer_vtol_manualswitch_autotransition_controller`: new manual switch controller for transitions starting from this profile. OFF keeps older manual behavior; ON makes `MIXER TRANSITION` start one complete transition. +- `mixer_vtol_transition_airspeed_timeout_ms`: new airspeed wait limit for transitions starting from this profile. It does not complete transition; it aborts an airspeed-controlled attempt that takes too long while pitot remains trusted. +- `mixer_vtol_transition_scale_ramp_time_ms`: new time used for smooth pusher/lift/transition-servo movement in transitions starting from this profile. It also controls how long transition-linked servo outputs continue from their current position after a direct switch or abort. + +Global VTOL transition settings: + +- `vtol_transition_to_fw_min_airspeed_cm_s`: new preferred MC -> FW completion threshold when pitot is trusted. `0` uses the timer path. +- `vtol_transition_to_mc_max_airspeed_cm_s`: new preferred FW -> MC completion threshold when pitot is trusted. `0` uses the timer path. +- `vtol_fw_to_mc_auto_switch_airspeed_cm_s`: new low-speed fixed-wing safety fallback. When non-zero, FW flight can automatically start FW -> MC if the usable transition airspeed source falls too low. In manual FW flight it requires the manual auto-transition controller. In mission/RTH/failsafe it requires `mixer_automated_switch = ON` and keeps the current navigation task in MC after the switch. +- `vtol_autotransition_always`: new optional two-position manual workflow. When ON and armed, selecting the other VTOL mixer profile starts the auto-transition controller instead of switching profiles directly. Disarmed direct profile switching remains available for bench/preflight checks. OFF keeps direct manual profile switching available when `MIXER TRANSITION` is not active. +- `vtol_transition_lift_min_percent`: new lowest lift motor power during dynamic transition scaling. `100` keeps full lift power. +- `vtol_transition_mc_authority_min_percent`: new lowest MC motor stabilisation strength during dynamic transition scaling. `100` keeps full MC stabilisation. +- `vtol_transition_fw_authority_min_percent`: new lowest FW control strength during dynamic transition scaling. It also scales optional target fixed-wing servo preview rules. +- `nav_vtol_mission_transition_user_action`: new mission selector. OFF disables mission transition; USER1..USER4 chooses which waypoint USER bit requests MC or FW. +- `nav_vtol_mission_transition_min_altitude_cm`: new minimum-altitude gate before mission-requested transition may start. `0` disables this gate. +- `nav_vtol_transition_retry_on_airspeed_timeout`: new mission retry option after an airspeed timeout. If retry also fails, the configured fail action is used. +- `nav_vtol_transition_fail_action_mc_to_fw`: new MC -> FW mission failure action after transition cannot complete safely. +- `nav_vtol_transition_fail_action_fw_to_mc`: new FW -> MC mission failure action after transition cannot complete safely. + +Global VTOL MC protection and landing settings: + +- `vtol_mc_protection_mode`: new master switch for VTOL MC protection. OFF changes nothing; NAV protects navigation/altitude behavior; NAV_AND_STABILIZED also shapes ANGLE/HORIZON roll, pitch, and yaw commands at speed. +- `vtol_mc_thr_reserve_percent`: new throttle reserve for attitude authority while altitude/NAV owns throttle. Applied before altitude anti-windup bounds. +- `nav_mc_hover_thr`: existing MC hover throttle hint. New VTOL protection use is to keep hover throttle inside the protected range and to make landing throttle confirmation more accurate. +- `nav_wp_radius`: existing normal waypoint acceptance radius. New VTOL landing use is capped internally for landing settle so a large waypoint radius cannot by itself start landing too early. +- `nav_wp_enforce_altitude`: existing waypoint altitude acceptance tolerance. New VTOL mission use is to keep transition waypoints from being accepted horizontally while still far from target altitude. +- `nav_rth_allow_landing`: existing RTH landing permission. VTOL landing logic only matters when navigation is actually allowed to land. +- `nav_disarm_on_landing`: existing automatic disarm permission. New VTOL MC landing confirmation must pass before this can lead to disarm. +- `nav_auto_disarm_delay`: existing delay after landing detection. VTOL MC adds its own confirmation before landing is reported. +- `nav_land_detect_sensitivity`: existing generic landing detector sensitivity. New VTOL MC use allows it to create candidates, but not to bypass vertical-speed and throttle confirmation. +- `nav_landing_bump_detection`: existing touchdown-bump candidate detection. New VTOL MC use treats bumps as candidates only, not immediate proof of landing. +- `nav_land_minalt_vspd`: existing final RTH landing descent speed. New VTOL MC use is a conservative vertical-speed reference for final settle/landing checks. +- `nav_land_maxalt_vspd`: existing higher-altitude RTH landing descent speed. It controls descent energy before the final slowdown region. +- `nav_land_slowdown_minalt`: existing lower boundary of the RTH landing slowdown window. Below this, RTH landing should be using `nav_land_minalt_vspd`. +- `nav_land_slowdown_maxalt`: existing upper boundary of the RTH landing slowdown window. Above this, RTH landing may use `nav_land_maxalt_vspd`. + +Mission profile-switch dependency: + +- Mission VTOL transition uses the existing profile-change path. +- Configure two valid mixer profiles. +- Configure a valid `MIXER PROFILE 2` mode activation condition. +- Enable `mixer_automated_switch` in profiles where NAV is allowed to request VTOL profile changes. # Notes and Experiences ## General @@ -297,3 +1300,58 @@ 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 + +## Airspeed-based transition logic (reference) + +When a usable transition airspeed source is available, transition progress is airspeed-driven, not timer-driven. A usable source is either a valid real pitot sensor or `pitot_hardware = VIRTUAL` with a valid virtual airspeed estimate. If no usable airspeed source is available, INAV uses `mixer_switch_trans_timer`. + +- 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 when a usable airspeed source is being used: + +- MC -> FW: + - forward motor power ramps `0 -> 1` + - lift motor power ramps `1 -> vtol_transition_lift_min_percent` + - MC stabilisation ramps `1 -> vtol_transition_mc_authority_min_percent` + - FW control ramps `vtol_transition_fw_authority_min_percent -> 1` + +- FW -> MC: + - forward motor power ramps `1 -> 0` + - lift motor power ramps `vtol_transition_lift_min_percent -> 1` + - MC stabilisation ramps `vtol_transition_mc_authority_min_percent -> 1` + - FW control ramps `1 -> vtol_transition_fw_authority_min_percent` + +After the profile switch, INAV keeps only the old propulsion output alive for a short smooth shutdown: + +- MC -> FW: lift motors that are not used by the FW profile move to idle. +- FW -> MC: a forward motor that is not used by the MC profile keeps moving to idle. +- This after-switch smoothing uses `mixer_vtol_transition_scale_ramp_time_ms` and does not keep the old PID/controller active. + +Forward motor power increase, transition servo movement, and airspeed-linked control scaling are related but separate. +For MC->FW, forward motor power uses `mixer_vtol_transition_scale_ramp_time_ms`; if this is `0`, the motor goes to full power immediately. +For FW->MC, the same timer ramps the forward motor down to idle while lift power and MC stabilisation rise back from their configured minimums; if this is `0`, those changes happen immediately. +For `INPUT_MIXER_TRANSITION`, the same timer is used only when `mixer_vtol_transition_dynamic_mixer = ON`. With dynamic mixer OFF, source 38 keeps the older fixed transition endpoint behavior. +If a profile switch or direct switch affects servos that use transition-linked inputs, INAV captures the current servo output and moves from that captured output toward the new profile output using a fresh `mixer_vtol_transition_scale_ramp_time_ms` window. +This timer does not decide when the transition completes. +In MC->FW, lift power reduction, MC stabilisation reduction, and FW control increase still prefer airspeed-based transition progress whenever a usable airspeed source is available. +In FW->MC, FW control reduction still prefers airspeed-based transition progress, while forward motor removal, lift power return, and MC motor stabilisation return use the time-based motor ramp. +If no usable transition airspeed source is available, the airspeed-linked changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). + +For legacy/helper transition motors (`-2.0 <= throttle <= -1.05`), output is interpolated from idle to target: + +`motor = idle + (target - idle) * pusherScale` + +where: +- `target = -mixerThrottle * 1000` +- `idle = throttleRangeMin` + +If no usable transition airspeed source is available, timer fallback is used (`mixer_switch_trans_timer`). + +For smooth auto-transition, the preferred forward motor setup is a normal positive-throttle rule in the FW mixer profile, with a placeholder on the same motor index in the MC mixer profile. Use `throttle = -1.000` for that placeholder if Configurator removes zero-throttle motor rules. If a helper such as `throttle = -1.200` is used, INAV moves from that helper output to the real FW mixer output after the profile switch. diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f4ab6479a5f..e0556afb646 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -620,6 +620,9 @@ main_sources(COMMON_SRC navigation/navigation_pos_estimator_flow.c navigation/navigation_private.h navigation/navigation_rover_boat.c + navigation/navigation_vtol_mc_protection.c + navigation/navigation_vtol_mc_protection.h + navigation/navigation_vtol_mc_protection_logic.h navigation/navigation_geozone.c navigation/sqrt_controller.c navigation/sqrt_controller.h diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 75ab075feac..ef3fb140d15 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" @@ -1373,10 +1374,32 @@ static void writeSlowFrame(void) */ static void loadSlowState(blackboxSlowState_t *slow) { +#ifdef USE_AUTO_TRANSITION + boxBitmask_t reportedRcModeFlags = rcModeActivationMask; +#endif + slow->activeWpNumber = getActiveWpNumber(); +#ifdef USE_AUTO_TRANSITION + // Keep these two mode bits aligned with actual VTOL state/profile activity for status reporting. + if (isMixerProfile2ModeReportedActive()) { + bitArraySet(reportedRcModeFlags.bits, BOXMIXERPROFILE); + } 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 +#else slow->rcModeFlags = rcModeActivationMask.bits[0]; // first 32 bits of boxId_e slow->rcModeFlags2 = rcModeActivationMask.bits[1]; // remaining bits of boxId_e +#endif // Also log Nav auto enabled flight modes rather than just those selected by boxmode if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) { diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b33868af8b2..fe1f127b9f3 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -80,6 +80,8 @@ typedef enum { DEBUG_LULU, DEBUG_SBUS2, DEBUG_OSD_REFRESH, + DEBUG_VTOL_TRANSITION, + DEBUG_VTOL_MC_PROTECT, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/common/log.c b/src/main/common/log.c index d1d9ef1e6b0..89b82534c55 100644 --- a/src/main/common/log.c +++ b/src/main/common/log.c @@ -205,13 +205,13 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t { // Print lines of up to maxBytes bytes. We need 5 characters per byte // 0xAB[space|\n] - // charsPerByte/maxBytes must be true compile-time constants (not `const` - // locals) so the buffer size below is a real constant expression, not a VLA. + // These must be true compile-time constants so the buffer size below is + // a real constant expression, not a VLA. enum { - charsPerByte = 5, - maxBytes = 8, + LOG_BUFFER_HEX_CHARS_PER_BYTE = 5, + LOG_BUFFER_HEX_MAX_BYTES = 8, }; - char buf[LOG_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator + char buf[LOG_PREFIX_FORMATTED_SIZE + LOG_BUFFER_HEX_CHARS_PER_BYTE * LOG_BUFFER_HEX_MAX_BYTES + 1]; // +1 for the null terminator size_t bufPos = LOG_PREFIX_FORMATTED_SIZE; const uint8_t *inputPtr = buffer; @@ -223,7 +223,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t for (size_t ii = 0; ii < size; ii++) { tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]); - bufPos += charsPerByte; + bufPos += LOG_BUFFER_HEX_CHARS_PER_BYTE; if (bufPos == sizeof(buf)-1) { buf[bufPos-1] = '\n'; buf[bufPos] = '\0'; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 8ed295c2d3f..674964e4781 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -228,7 +228,9 @@ static const char *debugModeNames[DEBUG_COUNT] = { "GPS", "LULU", "SBUS2", - "OSD_REFRESH" + "OSD_REFRESH", + "VTOL_TRANSITION", + "VTOL_MC_PROTECT" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d3021317ae5..5e1c87ac145 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -103,7 +103,12 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES ); +// Keep PG version split because USE_AUTO_TRANSITION changes the stored layout only on >512 KB targets. +#ifdef USE_AUTO_TRANSITION +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 11); +#else PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 7); +#endif PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, @@ -117,6 +122,17 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .i2c_speed = SETTING_I2C_SPEED_DEFAULT, #endif .throttle_tilt_compensation_strength = SETTING_THROTTLE_TILT_COMP_STR_DEFAULT, // 0-100, 0 - disabled +#ifdef USE_AUTO_TRANSITION + .vtolTransitionToFwMinAirspeed = SETTING_VTOL_TRANSITION_TO_FW_MIN_AIRSPEED_CM_S_DEFAULT, + .vtolTransitionToMcMaxAirspeed = SETTING_VTOL_TRANSITION_TO_MC_MAX_AIRSPEED_CM_S_DEFAULT, + .vtolFwToMcAutoSwitchAirspeed = SETTING_VTOL_FW_TO_MC_AUTO_SWITCH_AIRSPEED_CM_S_DEFAULT, + .vtolAutotransitionAlways = SETTING_VTOL_AUTOTRANSITION_ALWAYS_DEFAULT, + .vtolTransitionLiftMinPercent = SETTING_VTOL_TRANSITION_LIFT_MIN_PERCENT_DEFAULT, + .vtolTransitionMcAuthorityMinPercent = SETTING_VTOL_TRANSITION_MC_AUTHORITY_MIN_PERCENT_DEFAULT, + .vtolTransitionFwAuthorityMinPercent = SETTING_VTOL_TRANSITION_FW_AUTHORITY_MIN_PERCENT_DEFAULT, + .vtolMcProtectionMode = SETTING_VTOL_MC_PROTECTION_MODE_DEFAULT, + .vtolMcThrReservePercent = SETTING_VTOL_MC_THR_RESERVE_PERCENT_DEFAULT, +#endif .craftName = SETTING_NAME_DEFAULT, .pilotName = SETTING_NAME_DEFAULT ); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index e3bde5f3eb7..75abebf7f9f 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -31,6 +31,12 @@ #define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz +typedef enum { + VTOL_MC_PROTECTION_OFF = 0, + VTOL_MC_PROTECTION_NAV, + VTOL_MC_PROTECTION_NAV_AND_STABILIZED, +} vtolMcProtectionMode_e; + typedef enum { FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, @@ -78,6 +84,17 @@ typedef struct systemConfig_s { uint8_t i2c_speed; #endif uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. +#ifdef USE_AUTO_TRANSITION + uint16_t vtolTransitionToFwMinAirspeed; + uint16_t vtolTransitionToMcMaxAirspeed; + uint16_t vtolFwToMcAutoSwitchAirspeed; + bool vtolAutotransitionAlways; + uint8_t vtolTransitionLiftMinPercent; + uint8_t vtolTransitionMcAuthorityMinPercent; + uint8_t vtolTransitionFwAuthorityMinPercent; + uint8_t vtolMcProtectionMode; + uint8_t vtolMcThrReservePercent; +#endif char craftName[MAX_NAME_LENGTH + 1]; char pilotName[MAX_NAME_LENGTH + 1]; } systemConfig_t; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3223aca497e..91961bb7f0b 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -74,6 +74,7 @@ #include "msp/msp_serial.h" #include "navigation/navigation.h" +#include "navigation/navigation_vtol_mc_protection.h" #include "rx/rx.h" #include "rx/msp.h" @@ -418,14 +419,17 @@ static void processPilotAndFailSafeActions(float dT) } else { DEBUG_SET(DEBUG_RATE_DYNAMICS, 0, rcCommand[ROLL]); rcCommand[ROLL] = applyRateDynamics(rcCommand[ROLL], ROLL, dT); - DEBUG_SET(DEBUG_RATE_DYNAMICS, 1, rcCommand[ROLL]); DEBUG_SET(DEBUG_RATE_DYNAMICS, 2, rcCommand[PITCH]); rcCommand[PITCH] = applyRateDynamics(rcCommand[PITCH], PITCH, dT); - DEBUG_SET(DEBUG_RATE_DYNAMICS, 3, rcCommand[PITCH]); DEBUG_SET(DEBUG_RATE_DYNAMICS, 4, rcCommand[YAW]); rcCommand[YAW] = applyRateDynamics(rcCommand[YAW], YAW, dT); + + navigationVtolMcProtectionApplyStabilizedCommandShaping(&rcCommand[ROLL], &rcCommand[PITCH], &rcCommand[YAW]); + + DEBUG_SET(DEBUG_RATE_DYNAMICS, 1, rcCommand[ROLL]); + DEBUG_SET(DEBUG_RATE_DYNAMICS, 3, rcCommand[PITCH]); DEBUG_SET(DEBUG_RATE_DYNAMICS, 5, rcCommand[YAW]); } @@ -464,6 +468,7 @@ void disarm(disarmReason_t disarmReason) #endif statsOnDisarm(); logicConditionReset(); + navigationVtolMcProtectionResetTransientStates(); #ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 842b56563a0..a7e24e2ff59 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -447,8 +447,13 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)), BOXMULTIFUNCTION); #endif #if (MAX_MIXER_PROFILE_COUNT > 1) +#ifdef USE_AUTO_TRANSITION + CHECK_ACTIVE_BOX(IS_ENABLED(isMixerProfile2ModeReportedActive()), BOXMIXERPROFILE); + CHECK_ACTIVE_BOX(IS_ENABLED(isMixerTransitionModeReportedActive()), BOXMIXERTRANSITION); +#else CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION); +#endif #endif CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD); #ifdef USE_SERIAL_GIMBAL diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7d2e9c01385..96e597ca741 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,9 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", - "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH"] + "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "VTOL_TRANSITION", "VTOL_MC_PROTECT"] + - name: vtol_mc_protection_mode + values: ["OFF", "NAV", "NAV_AND_STABILIZED"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e @@ -171,6 +173,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 @@ -1270,16 +1281,42 @@ groups: field: mixer_config.controlProfileLinking type: bool - name: mixer_automated_switch - description: "If set to on, This mixer_profile will try to switch to another mixer_profile when 1.RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius on mixer_profile is a MULTIROTOR or TRICOPTER platform_type. 2. RTH landing is requested on this mixer_profile is a AIRPLANE platform_type" + description: "Allows navigation code to request VTOL mixer profile changes from this profile. In legacy RTH this lets MC/TRICOPTER profiles switch toward FW when returning from far away, and lets AIRPLANE profiles switch toward MC for RTH landing. With the VTOL auto-transition controller, mission waypoints can also request MC/FW transitions through their USER action. Enable this in each profile where NAV is allowed to change the mixer profile." default_value: OFF field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "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 a usable transition airspeed source is not available, INAV completes the transition from this timer instead. A usable transition airspeed source is a valid real pitot sensor, or `pitot_hardware = VIRTUAL` with a valid virtual airspeed estimate. With smooth VTOL transition power changes ON, airspeed-linked power and control changes also fall back to this timer whenever the transition airspeed source is not usable." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 max: 200 + - name: mixer_vtol_transition_dynamic_mixer + description: "Turns on smooth VTOL transition power and control scaling. In MC->FW it smoothly brings the forward motor in while lift power and multicopter stabilisation are reduced and fixed-wing control is increased. In FW->MC it smoothly removes the forward motor while target MC lift power and target MC motor stabilisation come back in, and fixed-wing control is reduced. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: OFF + field: mixer_config.vtolTransitionDynamicMixer + 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. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: OFF + field: mixer_config.manualVtolTransitionController + type: bool + - name: mixer_vtol_transition_airspeed_timeout_ms + description: "Maximum wait time [ms] for the required airspeed during an airspeed-controlled transition. The airspeed condition must be reached and remain satisfied for the internal 300 ms confirmation within this total time. This timer does not complete the transition; it only aborts it if the condition is not confirmed in time while the transition airspeed source remains usable. Real pitot and explicitly configured `pitot_hardware = VIRTUAL` can both be used. A source dropout shorter than 300 ms is ignored; persistent loss falls back to `mixer_switch_trans_timer`. Set to 0 to disable. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: mixer_config.vtolTransitionAirspeedTimeoutMs + min: 0 + max: 60000 + - name: mixer_vtol_transition_scale_ramp_time_ms + description: "Controls time-based smooth VTOL motor, power, and transition-servo movement. In MC->FW it moves the forward motor from idle to the requested power. After the switch to FW, old lift motors that are no longer used by the FW profile are moved to idle over the same time. In FW->MC it moves the forward motor down to idle while target MC lift power plus target MC motor stabilisation come back in. `INPUT_MIXER_TRANSITION` also uses this time when `mixer_vtol_transition_dynamic_mixer` is ON. If a profile switch or direct switch changes a transition-linked servo output, INAV starts a fresh smooth movement from the servo's current output using this full time value. `0` applies these time-based changes immediately. This timer does not decide when transition completes; completion still uses a usable transition airspeed source or `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: mixer_config.vtolTransitionScaleRampTimeMs + 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 @@ -2580,13 +2617,13 @@ groups: field: general.flags.disarm_on_landing type: bool - name: nav_land_detect_sensitivity - description: "Changes sensitivity of landing detection. Higher values increase speed of detection but also increase risk of false detection. Default value should work in most cases." + description: "Scales the generic landing detector velocity and gyro thresholds. `5` is nominal sensitivity: for MC this is about 100 cm/s horizontal and vertical speed plus 4 deg/s average pitch/roll gyro rate. Higher values relax these thresholds and can detect landing sooner, but increase false-detect risk. VTOL MC landing detection also uses additional hard vertical-speed and throttle-probe safety gates that this setting does not bypass." default_value: 5 field: general.land_detect_sensitivity min: 1 max: 15 - name: nav_landing_bump_detection - description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS)." + description: "Allows landing detection to use a touchdown G-bump candidate when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS). For VTOL MC mode the G-bump is not an immediate disarm shortcut: trusted high AGL blocks it, and accepted candidates must still pass the VTOL MC throttle-probe confirmation." default_value: OFF field: general.flags.landing_bump_detection type: bool @@ -2629,7 +2666,7 @@ groups: min: 10 max: 10000 - name: nav_wp_enforce_altitude - description: "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." + description: "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. After a successful mission VTOL transition from an ordinary WAYPOINT, a temporary transition-induced climb does not require loitering back down; the mission advances unless the aircraft is below the target altitude minus this tolerance. HOLD_TIME and LAND waypoints keep their normal behavior." default_value: 0 field: general.waypoint_enforce_altitude min: 0 @@ -2645,6 +2682,37 @@ 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, a working `MIXER PROFILE 2` mode setup, and a target with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: "OFF" + field: general.vtol_mission_transition_user_action + table: nav_wp_user_action + - name: nav_vtol_mission_transition_min_altitude_cm + description: "Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the altitude check. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: general.vtol_mission_transition_min_altitude + min: 0 + max: 50000 + - name: nav_vtol_transition_retry_on_airspeed_timeout + description: "If ON, INAV gets one extra MC->FW attempt after an airspeed timeout during mission or RTH. It pauses, yaws around to find the best airspeed direction, then tries once more. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: OFF + field: general.vtol_transition_retry_on_airspeed_timeout + type: bool + - name: nav_vtol_transition_fail_action_mc_to_fw + description: "What INAV should do if MC->FW transition still fails after the final attempt. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: "IDLE" + field: general.vtol_transition_fail_action_mc_to_fw + table: nav_vtol_transition_fail_action_mc_to_fw + - name: nav_vtol_transition_fail_action_fw_to_mc + description: "What INAV should do if FW->MC transition fails. `LOITER` keeps the aircraft near its current position. `FORCE_SWITCH` changes to the other mixer profile immediately even though the normal switch conditions were not met. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: "LOITER" + field: general.vtol_transition_fail_action_fw_to_mc + table: nav_vtol_transition_fail_action_fw_to_mc - 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 @@ -4069,6 +4137,67 @@ groups: field: throttle_tilt_compensation_strength min: 0 max: 100 + - name: vtol_transition_to_fw_min_airspeed_cm_s + description: "Minimum airspeed [cm/s] needed before MC->FW transition is considered complete while a usable transition airspeed source is available. The speed must remain at or above this value for the internal 300 ms confirmation. A usable source is a valid real pitot sensor, or `pitot_hardware = VIRTUAL` with a valid virtual airspeed estimate. If no usable source is available, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If the source remains usable but this target is not confirmed before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: vtolTransitionToFwMinAirspeed + min: 0 + max: 20000 + - name: vtol_transition_to_mc_max_airspeed_cm_s + description: "When slowing down from FW to MC, the transition is considered complete once airspeed remains at this value [cm/s] or lower for the internal 300 ms confirmation while a usable transition airspeed source is available. A usable source is a valid real pitot sensor, or `pitot_hardware = VIRTUAL` with a valid virtual airspeed estimate. If no usable source is available, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If the source remains usable but this condition is not confirmed before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: vtolTransitionToMcMaxAirspeed + min: 0 + max: 20000 + - name: vtol_fw_to_mc_auto_switch_airspeed_cm_s + description: "Extra low-speed fixed-wing safety fallback [cm/s]. If a usable transition airspeed source falls to this value or lower while in FW, INAV automatically starts FW->MC. Real pitot and explicitly configured `pitot_hardware = VIRTUAL` can both be used. In manual FW flight this requires `mixer_vtol_manualswitch_autotransition_controller` to be ON and keeps MC until the pilot deliberately commands another manual profile change. In mission/RTH/failsafe this requires `mixer_automated_switch` to be ON and keeps the current navigation task in MC after the safety switch, blocking automatic MC->FW re-entry for that navigation session. `vtol_transition_to_mc_max_airspeed_cm_s` still controls when the FW->MC transition is safe to complete. Set to 0 to disable. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: vtolFwToMcAutoSwitchAirspeed + min: 0 + max: 20000 + - name: vtol_autotransition_always + description: "When enabled, an armed manual VTOL profile switch between a multicopter-like profile and a fixed-wing profile starts the auto-transition controller instead of switching profiles directly. This allows a two-position desired-mode switch: selecting FW starts MC->FW, selecting MC starts FW->MC. Disarmed direct profile switching remains available for bench and preflight checks. OFF preserves direct manual profile switching when `MIXER TRANSITION` is not active. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: OFF + field: vtolAutotransitionAlways + type: bool + - name: vtol_transition_lift_min_percent + description: "Lowest lift motor base throttle used during transition, in percent. This scales the throttle/lift component of MC lift motors, not their roll/pitch/yaw stabilisation correction. In MC->FW, lift motor base throttle is reduced to this value as the transition progresses. In FW->MC, target MC lift motor base throttle starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift base throttle through the whole transition, but also starts FW->MC target lift at full base throttle. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 100 + field: vtolTransitionLiftMinPercent + min: 0 + max: 100 + - name: vtol_transition_mc_authority_min_percent + description: "Lowest multicopter stabilisation authority used during transition, in percent. This scales the MC roll/pitch/yaw correction mixed into MC motors, not the lift motor base throttle. In MC->FW, active MC motor stabilisation correction is reduced to this value as the transition progresses. In FW->MC, target MC motor stabilisation correction starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation correction through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 100 + field: vtolTransitionMcAuthorityMinPercent + min: 0 + max: 100 + - name: vtol_transition_fw_authority_min_percent + description: "Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength as the transition progresses. In FW->MC, it is reduced from full strength to this value as the transition progresses. With `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules configured in the MC mixer profile, this same setting scales their servo authority during MC->FW and scales down the matching FW servo stabilisation during FW->MC. `100` keeps full fixed-wing stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 100 + field: vtolTransitionFwAuthorityMinPercent + min: 0 + max: 100 + - name: vtol_mc_protection_mode + description: "Enables extra protection for VTOL aircraft while the active mixer profile is multicopter-like and another mixer profile is fixed-wing. OFF keeps legacy behavior. NAV protects altitude/NAV throttle authority, NAV capture/settle, landing settle, VTOL MC landing confirmation, and conservative bailout. NAV_AND_STABILIZED also applies speed-based roll/pitch/yaw command shaping in ANGLE/HORIZON when armed, VTOL MC mode is detected, velocity is trusted, and horizontal speed is high enough. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: "OFF" + field: vtolMcProtectionMode + table: vtol_mc_protection_mode + - name: vtol_mc_thr_reserve_percent + description: "Throttle range percentage reserved on both low and high side for VTOL MC attitude authority while navigation/altitude control owns throttle. The reserve is applied before altitude PID anti-windup bounds and is automatically shrunk if needed to keep hover throttle inside the usable range. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 15 + field: vtolMcThrReservePercent + min: 0 + max: 40 - name: name description: "Craft name" default_value: "" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 27f1ec7ffe2..249c03eafa7 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -48,6 +48,8 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" +#include "flight/mixer_transition_logic.h" #include "flight/pid.h" #include "flight/servos.h" @@ -264,6 +266,106 @@ void mixerResetDisarmedMotors(void) motor_disarmed[i] = motorZeroCommand; } } + +#ifdef USE_AUTO_TRANSITION +typedef struct autoTransitionMotorMixState_s { + bool active; + bool currentProfileIsMultirotor; + bool targetProfileIsMultirotor; + const motorMixer_t *targetMotorMixer; + const mixerConfig_t *targetMixerConfig; + int16_t targetInput[3]; +} autoTransitionMotorMixState_t; + +static uint16_t targetLiftMotorPreviewCaptureMask; +static int targetLiftMotorPreviewTargetProfileIndex = -1; +static mixerProfileATDirection_e targetLiftMotorPreviewDirection = MIXERAT_DIRECTION_NONE; +static int16_t targetLiftMotorPreviewCapturedOutput[MAX_SUPPORTED_MOTORS]; + +static void resetTargetLiftMotorPreviewCapture(void) +{ + targetLiftMotorPreviewCaptureMask = 0; + targetLiftMotorPreviewTargetProfileIndex = -1; + targetLiftMotorPreviewDirection = MIXERAT_DIRECTION_NONE; + memset(targetLiftMotorPreviewCapturedOutput, 0, sizeof(targetLiftMotorPreviewCapturedOutput)); +} + +// Keep transition-only session bookkeeping out of mixTable's F7 ITCM hot path. +static void NOINLINE updateTargetLiftMotorPreviewSession(const autoTransitionMotorMixState_t *state) +{ + if (!state->active || + mixerProfileAT.phase != MIXERAT_PHASE_TRANSITIONING || + mixerProfileAT.direction != MIXERAT_DIRECTION_TO_MC || + !currentMixerConfig.vtolTransitionDynamicMixer || + currentMixerConfig.vtolTransitionScaleRampTimeMs == 0 || + state->currentProfileIsMultirotor || + !state->targetProfileIsMultirotor || + nextMixerProfileIndex < 0 || + nextMixerProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + resetTargetLiftMotorPreviewCapture(); + return; + } + + if (targetLiftMotorPreviewTargetProfileIndex != nextMixerProfileIndex || + targetLiftMotorPreviewDirection != mixerProfileAT.direction) { + targetLiftMotorPreviewCaptureMask = 0; + targetLiftMotorPreviewTargetProfileIndex = nextMixerProfileIndex; + targetLiftMotorPreviewDirection = mixerProfileAT.direction; + memset(targetLiftMotorPreviewCapturedOutput, 0, sizeof(targetLiftMotorPreviewCapturedOutput)); + } +} + +static int16_t NOINLINE getTargetLiftMotorPreviewOutput(uint8_t motorIndex, int16_t currentOutput, int16_t targetOutput) +{ + const uint16_t motorMask = 1U << motorIndex; + if (!(targetLiftMotorPreviewCaptureMask & motorMask)) { + targetLiftMotorPreviewCapturedOutput[motorIndex] = currentOutput; + targetLiftMotorPreviewCaptureMask |= motorMask; + } + + return mixerTransitionBlendCapturedMotorOutput( + targetLiftMotorPreviewCapturedOutput[motorIndex], + targetOutput, + mixerProfileAT.motorRampProgress); +} + +static void NOINLINE prepareAutoTransitionMotorMixState(autoTransitionMotorMixState_t *state) +{ + if (!(isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.phase == MIXERAT_PHASE_TRANSITIONING && + mixerProfileAT.direction != MIXERAT_DIRECTION_NONE && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT)) { + return; + } + + state->active = true; + state->currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); + state->targetMotorMixer = mixerMotorMixersByIndex(nextMixerProfileIndex); + state->targetMixerConfig = mixerConfigByIndex(nextMixerProfileIndex); + state->targetProfileIsMultirotor = isMultirotorTypePlatform(state->targetMixerConfig->platformType); + + const float targetAuthorityScale = state->targetProfileIsMultirotor ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + + if (!state->targetProfileIsMultirotor && FLIGHT_MODE(MANUAL_MODE)) { + state->targetInput[ROLL] = rcCommand[ROLL]; + state->targetInput[PITCH] = rcCommand[PITCH]; + state->targetInput[YAW] = rcCommand[YAW]; + } else { + state->targetInput[ROLL] = getAutoTransitionTargetAxisPID(FD_ROLL); + state->targetInput[PITCH] = getAutoTransitionTargetAxisPID(FD_PITCH); + state->targetInput[YAW] = getAutoTransitionTargetAxisPID(FD_YAW); + } + + state->targetInput[ROLL] = state->targetInput[ROLL] * (state->targetMixerConfig->transition_PID_mmix_multiplier_roll / 1000.0f) * targetAuthorityScale; + state->targetInput[PITCH] = state->targetInput[PITCH] * (state->targetMixerConfig->transition_PID_mmix_multiplier_pitch / 1000.0f) * targetAuthorityScale; + state->targetInput[YAW] = state->targetInput[YAW] * (state->targetMixerConfig->transition_PID_mmix_multiplier_yaw / 1000.0f) * targetAuthorityScale; +} +#endif + #if !defined(SITL_BUILD) static uint16_t handleOutputScaling( int16_t input, // Input value from the mixer @@ -520,13 +622,31 @@ void FAST_CODE mixTable(void) input[ROLL] = axisPID[ROLL]; input[PITCH] = axisPID[PITCH]; input[YAW] = axisPID[YAW]; - if(isMixerTransitionMixing){ + if (isMixerTransitionMixing) { +#ifdef USE_AUTO_TRANSITION + const float activeAuthorityScale = isMultirotorTypePlatform(currentMixerConfig.platformType) ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f) * activeAuthorityScale; + input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f) * activeAuthorityScale; + input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f) * activeAuthorityScale; +#else input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f); input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f); input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f); +#endif } } +#ifdef USE_AUTO_TRANSITION + autoTransitionMotorMixState_t autoTransition = { 0 }; + + if (isMixerTransitionMixing) { + prepareAutoTransitionMotorMixState(&autoTransition); + } + updateTargetLiftMotorPreviewSession(&autoTransition); +#endif + // Initial mixer concept by bdoiron74 reused and optimized for Air Mode int16_t rpyMix[MAX_SUPPORTED_MOTORS]; int16_t rpyMixMax = 0; // assumption: symetrical about zero. @@ -534,10 +654,43 @@ void FAST_CODE mixTable(void) // motors for non-servo mixes for (int i = 0; i < motorCount; i++) { +#ifdef USE_AUTO_TRANSITION + const motorMixer_t *targetMixer = autoTransition.active ? &autoTransition.targetMotorMixer[i] : NULL; + const bool currentMotorActive = currentMixer[i].throttle > 0.0f; + const bool targetMotorActive = targetMixer && targetMixer->throttle > 0.0f; + const float activeRpyMix = currentMotorActive ? + (input[PITCH] * currentMixer[i].pitch + + input[ROLL] * currentMixer[i].roll + + -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) : + 0; + const float targetRpyMix = targetMotorActive ? + (autoTransition.targetInput[PITCH] * targetMixer->pitch + + autoTransition.targetInput[ROLL] * targetMixer->roll + + -motorYawMultiplier * autoTransition.targetInput[YAW] * targetMixer->yaw) : + 0; + float sharedRpyNormalizer = 1.0f; + + if (currentMotorActive && targetMotorActive) { + const float activeAuthorityScale = autoTransition.currentProfileIsMultirotor ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + const float targetAuthorityScale = autoTransition.targetProfileIsMultirotor ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + const float authorityScaleSum = activeAuthorityScale + targetAuthorityScale; + + if (authorityScaleSum > 1.0f) { + sharedRpyNormalizer = 1.0f / authorityScaleSum; + } + } + + rpyMix[i] = (activeRpyMix + targetRpyMix) * sharedRpyNormalizer * mixerScale; +#else rpyMix[i] = (input[PITCH] * currentMixer[i].pitch + input[ROLL] * currentMixer[i].roll + -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale; +#endif if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i]; if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i]; @@ -624,8 +777,42 @@ void FAST_CODE mixTable(void) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. +#ifdef USE_AUTO_TRANSITION + const float transitionPusherScale = isMixerTransitionMixing ? mixerATGetPusherScale() : 1.0f; +#endif for (int i = 0; i < motorCount; i++) { - motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax); + float motorThrottle = mixerThrottleCommand * currentMixer[i].throttle; +#ifdef USE_AUTO_TRANSITION + const motorMixer_t *targetMixer = autoTransition.active ? &autoTransition.targetMotorMixer[i] : NULL; + const bool currentMotorActive = currentMixer[i].throttle > 0.0f; + const bool targetMotorActive = targetMixer && targetMixer->throttle > 0.0f; + const bool sharedMotor = currentMotorActive && targetMotorActive; + const bool currentMotorPlaceholder = currentMixer[i].throttle <= 0.0f && currentMixer[i].throttle > -1.05f; + + if (autoTransition.active && currentMotorActive) { + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && autoTransition.currentProfileIsMultirotor) { + if (sharedMotor && !autoTransition.targetProfileIsMultirotor) { + const float propulsionBlend = transitionPusherScale; + const float blendedThrottle = currentMixer[i].throttle * (1.0f - propulsionBlend) + + targetMixer->throttle * propulsionBlend; + motorThrottle = mixerThrottleCommand * blendedThrottle; + } else { + motorThrottle *= mixerATGetLiftScale(); + } + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && !autoTransition.currentProfileIsMultirotor) { + if (sharedMotor && autoTransition.targetProfileIsMultirotor) { + const float mcBlend = 1.0f - transitionPusherScale; + const float blendedThrottle = currentMixer[i].throttle * transitionPusherScale + + targetMixer->throttle * mcBlend; + motorThrottle = mixerThrottleCommand * blendedThrottle; + } else { + motorThrottle *= transitionPusherScale; + } + } + } +#endif + + motor[i] = rpyMix[i] + constrain(motorThrottle, throttleMin, throttleMax); if (failsafeIsActive()) { motor[i] = constrain(motor[i], motorConfig()->mincommand, getMaxThrottle()); @@ -637,11 +824,54 @@ void FAST_CODE mixTable(void) if (currentMixer[i].throttle <= 0.0f) { motor[i] = motorZeroCommand; } +#ifdef USE_AUTO_TRANSITION + if (autoTransition.active && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && + autoTransition.currentProfileIsMultirotor && + currentMotorPlaceholder && + targetMotorActive && + !autoTransition.targetProfileIsMultirotor) { + // Smooth auto-transition pusher preview comes from the target FW + // mixer rule. A zero current rule reserves the physical motor index. + const float targetMotorThrottle = mixerThrottleCommand * targetMixer->throttle * transitionPusherScale; + motor[i] = constrain(rpyMix[i] + constrain(targetMotorThrottle, throttleMin, throttleMax), throttleRangeMin, throttleRangeMax); + } else if (autoTransition.active && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && + !autoTransition.currentProfileIsMultirotor && + currentMotorPlaceholder && + targetMotorActive && + autoTransition.targetProfileIsMultirotor) { + // Target MC lift preview uses the target MC mixer rule and target + // MC PID preview. This avoids feeding active FW/PIFF PID into lift + // motors while still allowing MC control to fade in before switch. + const float targetMotorThrottle = mixerThrottleCommand * targetMixer->throttle * mixerATGetLiftScale(); + const int16_t targetMotorOutput = constrain(rpyMix[i] + constrain(targetMotorThrottle, throttleMin, throttleMax), throttleRangeMin, throttleRangeMax); + motor[i] = getTargetLiftMotorPreviewOutput(i, motor[i], targetMotorOutput); + } +#endif //spin stopped motors only in mixer transition mode - if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && !feature(FEATURE_REVERSIBLE_MOTORS)) { + if (isMixerTransitionMixing && +#ifdef USE_AUTO_TRANSITION + mixerProfileAT.phase != MIXERAT_PHASE_TAILSITTER_TO_MC_CAPTURE && +#endif + currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && + !feature(FEATURE_REVERSIBLE_MOTORS)) { +#ifdef USE_AUTO_TRANSITION + const float pusherTarget = -currentMixer[i].throttle * 1000.0f; + const float pusherIdle = throttleRangeMin; + motor[i] = pusherIdle + (pusherTarget - pusherIdle) * transitionPusherScale; +#else motor[i] = -currentMixer[i].throttle * 1000; +#endif motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } +#ifdef USE_AUTO_TRANSITION + int16_t postSwitchFadeOutput = 0; + if (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE && + mixerATGetPostSwitchFadeMotorOutput(i, motorZeroCommand, motor[i], &postSwitchFadeOutput)) { + motor[i] = constrain(postSwitchFadeOutput, motorZeroCommand, getMaxThrottle()); + } +#endif } } @@ -742,4 +972,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 5f9caf165e6..9a9280f942d 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..956bb475334 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -1,3 +1,4 @@ +#include #include #include #include @@ -10,13 +11,20 @@ #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" #include "drivers/time.h" +#include "common/maths.h" +#include "common/utils.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" +#include "flight/mixer_transition_logic.h" #include "common/axis.h" #include "flight/pid.h" #include "flight/servos.h" #include "flight/failsafe.h" +#include "flight/imu.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 +37,12 @@ #include "navigation/navigation.h" #include "common/log.h" +#include "build/debug.h" + +#ifdef USE_AUTO_TRANSITION +#define MIXER_TRANSITION_OSD_EVENT_DISPLAY_MS 3000 +#define MIXER_TRANSITION_MC_SPEED_HIGH_MARGIN_CM_S 100.0f +#endif mixerConfig_t currentMixerConfig; int currentMixerProfileIndex; @@ -36,8 +50,28 @@ bool isMixerTransitionMixing; bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; int nextMixerProfileIndex; +#ifdef USE_AUTO_TRANSITION +static bool manualTransitionModeWasActive; +static bool manualTransitionReadyForEdge = true; +static mixerTransitionManualSessionMode_e manualTransitionSessionMode; +static bool manualFwToMcProtectionLatched; +static int16_t mixerTransitionServoInput; +static mixerProfileATOsdEvent_e mixerATOsdEvent; +static timeMs_t mixerATOsdEventUntil; +static mixerProfileATDirection_e mixerATOsdSwitchReminderDirection; +static bool navigationProfileSwitchWasOwned; +static bool navigationProfileHandbackPending; +static bool manualProfileSwitchAutoTransitionActive; +static bool isTailSitterProfilePairConfigured(void); +static bool isTailSitterManualToMcCapture(void); +#endif +// Keep PG version split because USE_AUTO_TRANSITION changes the stored mixer profile layout only on >512 KB targets. +#ifdef USE_AUTO_TRANSITION +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 4); +#else PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); +#endif void pgResetFn_mixerProfiles(mixerProfile_t *instance) { @@ -53,6 +87,12 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .controlProfileLinking = SETTING_MIXER_CONTROL_PROFILE_LINKING_DEFAULT, .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, +#ifdef USE_AUTO_TRANSITION + .vtolTransitionDynamicMixer = SETTING_MIXER_VTOL_TRANSITION_DYNAMIC_MIXER_DEFAULT, + .manualVtolTransitionController = SETTING_MIXER_VTOL_MANUALSWITCH_AUTOTRANSITION_CONTROLLER_DEFAULT, + .vtolTransitionAirspeedTimeoutMs = SETTING_MIXER_VTOL_TRANSITION_AIRSPEED_TIMEOUT_MS_DEFAULT, + .vtolTransitionScaleRampTimeMs = SETTING_MIXER_VTOL_TRANSITION_SCALE_RAMP_TIME_MS_DEFAULT, +#endif .tailsitterOrientationOffset = SETTING_TAILSITTER_ORIENTATION_OFFSET_DEFAULT, .transition_PID_mmix_multiplier_roll = SETTING_TRANSITION_PID_MMIX_MULTIPLIER_ROLL_DEFAULT, .transition_PID_mmix_multiplier_pitch = SETTING_TRANSITION_PID_MMIX_MULTIPLIER_PITCH_DEFAULT, @@ -82,56 +122,899 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) } } -void activateMixerConfig(void){ - currentMixerProfileIndex = getConfigMixerProfile(); - currentMixerConfig = *mixerConfig(); - nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; +void activateMixerConfig(void){ + currentMixerProfileIndex = getConfigMixerProfile(); + currentMixerConfig = *mixerConfig(); + nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; +} + +void mixerConfigInit(void) +{ + activateMixerConfig(); + servosInit(); + mixerUpdateStateFlags(); + mixerInit(); + + if (currentMixerConfig.controlProfileLinking) { + setConfigProfile(getConfigMixerProfile()); + } + + // Reinitialize the active controller on every mixer hot-switch so AUTO PID + // selection follows the new platform type and no stale FW/MC integrator + // state leaks across the completed transition. + pidInit(); + pidInitFilters(); + pidResetErrorAccumulators(); + schedulePidGainsUpdate(); + navigationUsePIDs(); +} + +void setMixerProfileAT(void) +{ +#ifdef USE_AUTO_TRANSITION + const timeMs_t now = millis(); + + mixerProfileAT.transitionStartTime = now; + mixerProfileAT.aborted = false; + mixerProfileAT.directSwitchAbort = false; + mixerProfileAT.abortedByAirspeedTimeout = false; + mixerProfileAT.hotSwitchDone = false; + mixerProfileAT.usedAirspeed = false; + mixerProfileAT.tailSitterManualTransition = false; + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_NONE; + mixerProfileAT.transitionStartAirspeedCaptured = false; + mixerProfileAT.progress = 0.0f; + mixerProfileAT.handoffScalingProgress = 0.0f; + memset(&mixerProfileAT.airspeedProgressFilter, 0, sizeof(mixerProfileAT.airspeedProgressFilter)); + mixerProfileAT.motorRampProgress = 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; + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeFastToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); + mixerProfileAT.servoHandoffMask = 0; + mixerProfileAT.servoHandoffDurationMs = 0; + mixerProfileAT.servoHandoffHoldDurationMs = 0; + mixerProfileAT.servoHandoffStartTime = 0; + mixerProfileAT.servoHandoffHoldStartTime = 0; + mixerProfileAT.tailSitterCaptureStableSince = 0; + memset(mixerProfileAT.servoHandoffOutput, 0, sizeof(mixerProfileAT.servoHandoffOutput)); +#else + mixerProfileAT.transitionStartTime = millis(); + mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; +#endif +} + +#ifdef USE_AUTO_TRANSITION +static void setMixerATOsdEvent(const mixerProfileATOsdEvent_e event) +{ + mixerATOsdEvent = event; + mixerATOsdEventUntil = event == MIXERAT_OSD_EVENT_NONE ? 0 : millis() + MIXER_TRANSITION_OSD_EVENT_DISPLAY_MS; +} + +static mixerProfileATOsdEvent_e mixerATAbortedEventForCurrentProfile(void) +{ + if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) { + return MIXERAT_OSD_EVENT_ABORTED_IN_FW; + } + + if (isMultirotorTypePlatform(currentMixerConfig.platformType)) { + return MIXERAT_OSD_EVENT_ABORTED_IN_MC; + } + + return MIXERAT_OSD_EVENT_ABORTED; +} + +static void updateMixerATSwitchReminder( + const bool transitionModeActive, + const int requestedProfileIndex) +{ + mixerATOsdSwitchReminderDirection = mixerTransitionManualSwitchReminderDirection( + manualTransitionSessionMode, + mixerATIsActive(), + mixerProfileAT.hotSwitchDone, + transitionModeActive, + currentMixerProfileIndex, + requestedProfileIndex, + isMultirotorTypePlatform(currentMixerConfig.platformType)); + + if (mixerATOsdSwitchReminderDirection == MIXERAT_DIRECTION_NONE && navigationProfileHandbackPending) { + mixerATOsdSwitchReminderDirection = isMultirotorTypePlatform(currentMixerConfig.platformType) ? + MIXERAT_DIRECTION_TO_MC : + MIXERAT_DIRECTION_TO_FW; + } +} + +static void clearServoHandoffFade(void) +{ + mixerProfileAT.servoHandoffMask = 0; + mixerProfileAT.servoHandoffDurationMs = 0; + mixerProfileAT.servoHandoffHoldDurationMs = 0; + mixerProfileAT.servoHandoffStartTime = 0; + mixerProfileAT.servoHandoffHoldStartTime = 0; + memset(mixerProfileAT.servoHandoffOutput, 0, sizeof(mixerProfileAT.servoHandoffOutput)); +} + +static bool isServoTransitionLinkedInputSource(const uint8_t inputSource) +{ + return inputSource == INPUT_MIXER_TRANSITION || + (inputSource >= INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL && + inputSource <= INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS); +} + +static bool isServoAutoTransitionTargetInputSource(const uint8_t inputSource) +{ + return inputSource >= INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL && + inputSource <= INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS; +} + +static uint32_t collectServoTargetMask(const servoMixer_t *rules) +{ + uint32_t mask = 0; + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + if (rules[i].rate == 0) { + break; + } + + if (rules[i].targetChannel < MAX_SUPPORTED_SERVOS) { + mask |= 1U << rules[i].targetChannel; + } + } + + return mask; +} + +static uint32_t collectTransitionLinkedServoTargetMask(const servoMixer_t *rules) +{ + uint32_t mask = 0; + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + if (rules[i].rate == 0) { + break; + } + + if (rules[i].targetChannel >= MAX_SUPPORTED_SERVOS || + !isServoTransitionLinkedInputSource(rules[i].inputSource)) { + continue; + } + + mask |= 1U << rules[i].targetChannel; + } + + return mask; +} + +static uint32_t collectAutoTransitionTargetServoMask(const servoMixer_t *rules) +{ + uint32_t mask = 0; + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + if (rules[i].rate == 0) { + break; + } + + if (rules[i].targetChannel >= MAX_SUPPORTED_SERVOS || + !isServoAutoTransitionTargetInputSource(rules[i].inputSource)) { + continue; + } + + mask |= 1U << rules[i].targetChannel; + } + + return mask; +} + +static bool servoMixerRuleMatches(const servoMixer_t *lhs, const servoMixer_t *rhs) +{ + return lhs->targetChannel == rhs->targetChannel && + lhs->inputSource == rhs->inputSource && + lhs->rate == rhs->rate && + lhs->speed == rhs->speed +#ifdef USE_PROGRAMMING_FRAMEWORK + && lhs->conditionId == rhs->conditionId +#endif + ; +} + +static int collectServoRulesForTarget(const servoMixer_t *rules, const uint8_t targetChannel, const servoMixer_t *targetRules[MAX_SERVO_RULES]) +{ + int ruleCount = 0; + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + if (rules[i].rate == 0) { + break; + } + + if (rules[i].targetChannel != targetChannel) { + continue; + } + + targetRules[ruleCount++] = &rules[i]; + } + + return ruleCount; +} + +static bool servoTargetRulesDiffer(const servoMixer_t *currentRules, const servoMixer_t *targetRules, const uint8_t targetChannel) +{ + const servoMixer_t *currentTargetRules[MAX_SERVO_RULES]; + const servoMixer_t *nextTargetRules[MAX_SERVO_RULES]; + const int currentRuleCount = collectServoRulesForTarget(currentRules, targetChannel, currentTargetRules); + const int nextRuleCount = collectServoRulesForTarget(targetRules, targetChannel, nextTargetRules); + + if (currentRuleCount != nextRuleCount) { + return true; + } + + for (int i = 0; i < currentRuleCount; i++) { + if (!servoMixerRuleMatches(currentTargetRules[i], nextTargetRules[i])) { + return true; + } + } + + return false; +} + +static uint32_t collectServoProfileDifferenceMask(const servoMixer_t *currentRules, const servoMixer_t *targetRules) +{ + const uint32_t allTargetsMask = collectServoTargetMask(currentRules) | collectServoTargetMask(targetRules); + uint32_t differenceMask = 0; + + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + if ((allTargetsMask & (1U << i)) == 0) { + continue; + } + + if (servoTargetRulesDiffer(currentRules, targetRules, i)) { + differenceMask |= 1U << i; + } + } + + return differenceMask; +} + +static uint16_t getServoHandoffDurationMs(void); + +static uint32_t collectServoHandoffMask(const int targetProfileIndex, const bool includeProfileDifferences) +{ + const servoMixer_t *currentRules = mixerServoMixersByIndex(currentMixerProfileIndex); + uint32_t handoffMask = collectTransitionLinkedServoTargetMask(currentRules); + + if (targetProfileIndex < 0 || targetProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + return handoffMask; + } + + const servoMixer_t *targetRules = mixerServoMixersByIndex(targetProfileIndex); + + handoffMask |= collectTransitionLinkedServoTargetMask(targetRules); + + if (includeProfileDifferences) { + handoffMask |= collectServoProfileDifferenceMask(currentRules, targetRules); + } + + return handoffMask; +} + +static uint32_t collectTransitionEntryServoHandoffMask(const int targetProfileIndex) +{ + const servoMixer_t *currentRules = mixerServoMixersByIndex(currentMixerProfileIndex); + uint32_t handoffMask = collectAutoTransitionTargetServoMask(currentRules); + + if (targetProfileIndex < 0 || targetProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + return handoffMask; + } + + const servoMixer_t *targetRules = mixerServoMixersByIndex(targetProfileIndex); + handoffMask |= collectAutoTransitionTargetServoMask(targetRules); + + return handoffMask; +} + +static void prepareServoHandoffFade(const uint32_t handoffMask) +{ + clearServoHandoffFade(); + + const uint16_t handoffDurationMs = getServoHandoffDurationMs(); + + if (handoffMask == 0 || handoffDurationMs == 0) { + return; + } + + mixerProfileAT.servoHandoffMask = handoffMask; + mixerProfileAT.servoHandoffDurationMs = handoffDurationMs; + + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + if ((handoffMask & (1U << i)) == 0) { + continue; + } + + mixerProfileAT.servoHandoffOutput[i] = servo[i]; + } +} + +static void startServoHandoffFade(void) +{ + if (mixerProfileAT.servoHandoffMask == 0 || mixerProfileAT.servoHandoffDurationMs == 0) { + return; + } + + mixerProfileAT.servoHandoffHoldDurationMs = 0; + mixerProfileAT.servoHandoffHoldStartTime = 0; + mixerProfileAT.servoHandoffStartTime = millis(); +} + +static bool updateServoHandoffFadeExpiration(void) +{ + if (mixerProfileAT.servoHandoffMask == 0) { + return false; + } + + const uint32_t elapsedMs = millis() - mixerProfileAT.servoHandoffStartTime; + if (!mixerTransitionServoHandoffExpired(elapsedMs, mixerProfileAT.servoHandoffDurationMs)) { + return true; + } + + clearServoHandoffFade(); + return false; +} + +static void startTransitionEntryServoHandoffFade(void) +{ + // Smooth only target-preview surfaces at transition entry. Tilt servos that + // use INPUT_MIXER_TRANSITION already have their own transition trajectory. + prepareServoHandoffFade(collectTransitionEntryServoHandoffMask(nextMixerProfileIndex)); + startServoHandoffFade(); +} + +static bool outputProfileDirectSwitch(const int profileIndex, const bool immediateServoResponse) +{ + clearServoHandoffFade(); + if (immediateServoResponse) { + // VTOL direct switches must not inherit speed-limited servo carryover + // from the old profile; the target profile should take over immediately. + servoMixerSetCarryoverOnNextLoad(false); + } + const bool switched = outputProfileHotSwitch(profileIndex); + servoMixerSetCarryoverOnNextLoad(true); + clearServoHandoffFade(); + return switched; +} + +bool outputProfileDirectSwitchImmediate(const int profileIndex) +{ + return outputProfileDirectSwitch(profileIndex, true); +} + +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 || + required_action == MIXERAT_REQUEST_FW_TO_MC_PROTECTION) { + return MIXERAT_DIRECTION_TO_MC; + } + + return MIXERAT_DIRECTION_NONE; +} + +static void resetTransitionScales(void) +{ + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_NONE; + mixerProfileAT.progress = 0.0f; + mixerProfileAT.handoffScalingProgress = 0.0f; + memset(&mixerProfileAT.airspeedProgressFilter, 0, sizeof(mixerProfileAT.airspeedProgressFilter)); + mixerProfileAT.motorRampProgress = 0.0f; + mixerProfileAT.blendToFw = 0.0f; + mixerProfileAT.pusherScale = 0.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeFastToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); +} + +static void setLegacyTransitionScales(void) +{ + mixerProfileAT.progress = 1.0f; + mixerProfileAT.handoffScalingProgress = 1.0f; + memset(&mixerProfileAT.airspeedProgressFilter, 0, sizeof(mixerProfileAT.airspeedProgressFilter)); + mixerProfileAT.motorRampProgress = 1.0f; + mixerProfileAT.blendToFw = 1.0f; + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; + mixerProfileAT.postSwitchFadeProgress = 1.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeFastToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); +} + +static float getMotorRampProgress(void) +{ + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + mixerProfileAT.motorRampProgress = mixerTransitionComputeMotorRampProgress( + currentMixerConfig.vtolTransitionDynamicMixer, + currentMixerConfig.vtolTransitionScaleRampTimeMs, + elapsedMs); + return mixerProfileAT.motorRampProgress; +} + +static uint16_t getServoHandoffDurationMs(void) +{ + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + + return mixerTransitionComputeServoHandoffDurationMs( + currentMixerConfig.vtolTransitionDynamicMixer, + currentMixerConfig.vtolTransitionScaleRampTimeMs, + elapsedMs); +} + +static float updateHandoffScalingProgress(const timeMs_t currentTimeMs) +{ + mixerProfileAT.handoffScalingProgress = mixerTransitionResolveHandoffProgress( + currentMixerConfig.vtolTransitionDynamicMixer, + mixerProfileAT.usedAirspeed, + mixerProfileAT.handoffScalingProgress, + mixerProfileAT.progress, + currentTimeMs, + MIXER_TRANSITION_AIRSPEED_CONFIRM_MS, + MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_MS, + &mixerProfileAT.airspeedProgressFilter); + return mixerProfileAT.handoffScalingProgress; +} + +static bool hasUsableTransitionAirspeed(float *airspeedCmS) +{ +#ifdef USE_PITOT + if (!sensors(SENSOR_PITOT) || !pitotGetValidForAirspeed() || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE) { + return false; + } + + const float measuredAirspeedCmS = detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL ? + getAirspeedEstimate() : + pitot.airSpeed; + + if (isnan(measuredAirspeedCmS) || isinf(measuredAirspeedCmS)) { + return false; + } + + *airspeedCmS = MAX(measuredAirspeedCmS, 0.0f); + return true; +#else + UNUSED(airspeedCmS); + return false; +#endif +} + +static void preparePostSwitchFade(const int targetProfileIndex) +{ + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeFastToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); + + if (!currentMixerConfig.vtolTransitionDynamicMixer || + currentMixerConfig.vtolTransitionScaleRampTimeMs == 0 || + mixerProfileAT.direction == MIXERAT_DIRECTION_NONE || + targetProfileIndex < 0 || + targetProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + return; + } + + const motorMixer_t *currentMotorMixer = mixerMotorMixersByIndex(currentMixerProfileIndex); + const motorMixer_t *targetMotorMixer = mixerMotorMixersByIndex(targetProfileIndex); + const bool currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); + const uint8_t count = getMotorCount(); + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + currentMixerConfig.vtolTransitionDynamicMixer, + currentMixerConfig.vtolTransitionScaleRampTimeMs, + mixerProfileAT.direction, + currentProfileIsMultirotor, + count, + currentMotorMixer, + targetMotorMixer); + + mixerProfileAT.postSwitchFadeMotorMask = fadeMask.motorMask; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = fadeMask.toCurrentMotorMask; + mixerProfileAT.postSwitchFadeFastToCurrentMotorMask = fadeMask.fastToCurrentMotorMask; + + for (uint8_t i = 0; i < count && i < MAX_SUPPORTED_MOTORS; i++) { + if (mixerProfileAT.postSwitchFadeMotorMask & (1U << i)) { + mixerProfileAT.postSwitchFadeMotorOutput[i] = motor[i]; + } + } + + if (mixerProfileAT.postSwitchFadeMotorMask == 0) { + return; + } + + mixerProfileAT.postSwitchFadeDurationMs = mixerTransitionComputePostSwitchFadeDurationMs( + currentMixerConfig.vtolTransitionScaleRampTimeMs, + mixerProfileAT.postSwitchFadeMotorMask, + mixerProfileAT.postSwitchFadeFastToCurrentMotorMask); + mixerProfileAT.postSwitchFadeInitialScale = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW ? + constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) : + constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f); +} + +static bool startPostSwitchFade(void) +{ + if (mixerProfileAT.postSwitchFadeMotorMask == 0 || + mixerProfileAT.postSwitchFadeDurationMs == 0) { + return false; + } + + mixerProfileAT.postSwitchFadeStartTime = millis(); + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.phase = MIXERAT_PHASE_POST_SWITCH_FADE; + isMixerTransitionMixing_requested = false; + return true; +} + +static bool updatePostSwitchFade(void) +{ + if (mixerProfileAT.phase != MIXERAT_PHASE_POST_SWITCH_FADE) { + return true; + } + + if (mixerProfileAT.postSwitchFadeDurationMs == 0) { + mixerProfileAT.postSwitchFadeProgress = 1.0f; + } else { + const uint32_t elapsedMs = millis() - mixerProfileAT.postSwitchFadeStartTime; + mixerProfileAT.postSwitchFadeProgress = constrainf((float)elapsedMs / (float)mixerProfileAT.postSwitchFadeDurationMs, 0.0f, 1.0f); + } + + const float remainingScale = mixerProfileAT.postSwitchFadeInitialScale * (1.0f - mixerProfileAT.postSwitchFadeProgress); + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = remainingScale; + mixerProfileAT.mcAuthorityScale = 0.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; + mixerProfileAT.blendToFw = 1.0f; + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + mixerProfileAT.pusherScale = remainingScale; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 0.0f; + mixerProfileAT.blendToFw = 0.0f; + } + + if (mixerProfileAT.postSwitchFadeProgress < 1.0f) { + return false; + } + + setMixerATOsdEvent(MIXERAT_OSD_EVENT_DONE); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_NONE; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeFastToCurrentMotorMask = 0; + manualProfileSwitchAutoTransitionActive = false; + return true; +} + +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) || isTailSitterProfilePairConfigured()) { + return false; + } + + const uint16_t thresholdCmS = systemConfig()->vtolFwToMcAutoSwitchAirspeed; + if (thresholdCmS == 0) { + return false; + } + + float airspeedCmS = 0.0f; + if (!hasUsableTransitionAirspeed(&airspeedCmS)) { + return false; + } + + return mixerTransitionFwToMcProtectionTriggered(ARMING_FLAG(ARMED), STATE(AIRPLANE), thresholdCmS, true, airspeedCmS); +} + +static void updateTransitionScales(void) +{ + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + currentMixerConfig.vtolTransitionDynamicMixer, + mixerProfileAT.direction, + systemConfig()->vtolTransitionLiftMinPercent / 100.0f, + systemConfig()->vtolTransitionMcAuthorityMinPercent / 100.0f, + systemConfig()->vtolTransitionFwAuthorityMinPercent / 100.0f, + mixerProfileAT.handoffScalingProgress, + getMotorRampProgress()); + + mixerProfileAT.blendToFw = scales.blendToFw; + mixerProfileAT.pusherScale = scales.pusherScale; + mixerProfileAT.liftScale = scales.liftScale; + mixerProfileAT.mcAuthorityScale = scales.mcAuthorityScale; + mixerProfileAT.fwAuthorityScale = scales.fwAuthorityScale; +} + +static void abortTransition(const bool byAirspeedTimeout, const bool directSwitchAbort) +{ + const bool wasActive = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; + const bool hotSwitchAlreadyDone = mixerProfileAT.hotSwitchDone; + const uint32_t servoHandoffMask = (wasActive && !hotSwitchAlreadyDone && !directSwitchAbort) ? + collectServoHandoffMask(nextMixerProfileIndex, false) : + 0; + + if (servoHandoffMask != 0) { + prepareServoHandoffFade(servoHandoffMask); + } + + if (wasActive) { + setMixerATOsdEvent(byAirspeedTimeout ? MIXERAT_OSD_EVENT_AIRSPEED_TIMEOUT : mixerATAbortedEventForCurrentProfile()); + } + + isMixerTransitionMixing_requested = false; + manualProfileSwitchAutoTransitionActive = false; + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.aborted = wasActive; + mixerProfileAT.directSwitchAbort = wasActive && directSwitchAbort; + mixerProfileAT.abortedByAirspeedTimeout = wasActive && byAirspeedTimeout; + mixerProfileAT.hotSwitchDone = false; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + mixerProfileAT.usedAirspeed = false; + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_NONE; + mixerProfileAT.transitionStartAirspeedCaptured = false; + mixerProfileAT.transitionStartAirspeedCmS = 0.0f; + mixerProfileAT.tailSitterManualTransition = false; + mixerProfileAT.tailSitterCaptureStableSince = 0; + resetTransitionScales(); + + if (servoHandoffMask != 0) { + startServoHandoffFade(); + } +} + +static bool mixerATReadyForHotSwitch(const mixerProfileATRequest_e required_action) +{ + const mixerProfileATDirection_e direction = directionForRequest(required_action); + const uint16_t airspeedThresholdCmS = getAirspeedThresholdForDirection(direction); + const timeMs_t currentTimeMs = millis(); + const uint32_t elapsedMs = currentTimeMs - mixerProfileAT.transitionStartTime; + const uint32_t transitionTimerMs = MAX(0, currentMixerConfig.switchTransitionTimer) * 100; + float airspeedCmS = 0.0f; + const bool usableAirspeedAvailable = + airspeedThresholdCmS > 0 && hasUsableTransitionAirspeed(&airspeedCmS); + + if (isTailSitterManualToMcCapture() && !usableAirspeedAvailable) { + // Timer fallback is acceptable for the established tilt/pusher paths, + // but a tailsitter must not rotate into MC at an unknown airspeed. + mixerProfileAT.usedAirspeed = false; + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_NO_SPEED; + updateHandoffScalingProgress(currentTimeMs); + return false; + } + const mixerTransitionHotSwitchProgress_t hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + direction, + airspeedThresholdCmS, + usableAirspeedAvailable, + airspeedCmS, + mixerProfileAT.transitionStartAirspeedCaptured, + mixerProfileAT.transitionStartAirspeedCmS, + elapsedMs, + transitionTimerMs); + + const bool holdForAirspeedRecovery = + airspeedThresholdCmS > 0 && + !hotSwitchProgress.usedAirspeed && + mixerTransitionAirspeedProgressHasRecentSample( + &mixerProfileAT.airspeedProgressFilter, + currentTimeMs, + MIXER_TRANSITION_AIRSPEED_CONFIRM_MS); + + if (holdForAirspeedRecovery) { + // A single validity dropout must not turn an airspeed-controlled + // transition into an immediate timer hot-switch. Keep the last safe + // progress briefly; persistent loss still falls back to the timer. + mixerProfileAT.usedAirspeed = false; + mixerProfileAT.waitReason = direction == MIXERAT_DIRECTION_TO_MC ? + MIXERAT_WAIT_REASON_NO_SPEED : + MIXERAT_WAIT_REASON_NONE; + return false; + } + + mixerProfileAT.usedAirspeed = hotSwitchProgress.usedAirspeed; + mixerProfileAT.transitionStartAirspeedCaptured = hotSwitchProgress.transitionStartAirspeedCaptured; + mixerProfileAT.transitionStartAirspeedCmS = hotSwitchProgress.transitionStartAirspeedCmS; + mixerProfileAT.progress = hotSwitchProgress.progress; + + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_NONE; + if (!hotSwitchProgress.readyForHotSwitch && + direction == MIXERAT_DIRECTION_TO_MC && + airspeedThresholdCmS > 0) { + if (!usableAirspeedAvailable) { + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_NO_SPEED; + } else if (airspeedCmS > (float)airspeedThresholdCmS + MIXER_TRANSITION_MC_SPEED_HIGH_MARGIN_CM_S) { + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_MC_SPEED_HIGH; + } else { + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_MC_SPEED; + } + } + + updateHandoffScalingProgress(currentTimeMs); + + return hotSwitchProgress.usedAirspeed ? + mixerTransitionAirspeedProgressEndpointConfirmed(&mixerProfileAT.airspeedProgressFilter) : + hotSwitchProgress.readyForHotSwitch; +} +#endif + +bool platformTypeConfigured(flyingPlatformType_e platformType) +{ + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){ + return false; + } + return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType; } -void mixerConfigInit(void) +#ifdef USE_AUTO_TRANSITION +static bool missionTransitionToMultirotorTypeConfigured(void) { - activateMixerConfig(); - servosInit(); - mixerUpdateStateFlags(); - mixerInit(); - if (currentMixerConfig.controlProfileLinking) - { - // LOG_INFO(PWM, "mixer switch pidInit"); - setConfigProfile(getConfigMixerProfile()); - pidInit(); - pidInitFilters(); - pidResetErrorAccumulators(); //should be safer to reset error accumulators - schedulePidGainsUpdate(); - navigationUsePIDs(); // set navigation pid gains + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) { + return false; } + + const flyingPlatformType_e nextPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; + return isMultirotorTypePlatform(nextPlatformType); } +#endif -void setMixerProfileAT(void) +#ifdef USE_AUTO_TRANSITION +static bool isTailSitterProfilePairConfigured(void) { - mixerProfileAT.transitionStartTime = millis(); - mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; + if (nextMixerProfileIndex < 0 || nextMixerProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + return false; + } + + const mixerConfig_t *targetMixerConfig = mixerConfigByIndex(nextMixerProfileIndex); + return mixerTransitionIsTailSitterProfilePair( + currentMixerConfig.platformType == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(currentMixerConfig.platformType), + currentMixerConfig.tailsitterOrientationOffset, + targetMixerConfig->platformType == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(targetMixerConfig->platformType), + targetMixerConfig->tailsitterOrientationOffset); } -bool platformTypeConfigured(flyingPlatformType_e platformType) -{ - if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){ +static bool isTailSitterTransitionRequestSupported(const mixerProfileATRequest_e requiredAction) +{ + if (nextMixerProfileIndex < 0 || nextMixerProfileIndex >= MAX_MIXER_PROFILE_COUNT) { return false; } - return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType; + + return mixerTransitionTailSitterRequestIsSupported( + requiredAction, + isTailSitterProfilePairConfigured(), + currentMixerConfig.vtolTransitionDynamicMixer, + mixerConfigByIndex(nextMixerProfileIndex)->vtolTransitionDynamicMixer); } -bool checkMixerATRequired(mixerProfileATRequest_e required_action) +static bool isTailSitterManualToMcCapture(void) { - //return false if mixerAT condition is not required or setting is not valid - if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) - { + return mixerTransitionTailSitterNeedsMcCapture( + mixerProfileAT.request, + mixerProfileAT.tailSitterManualTransition); +} + +static void completeTransition(void) +{ + setMixerATOsdEvent(MIXERAT_OSD_EVENT_DONE); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + mixerProfileAT.waitReason = MIXERAT_WAIT_REASON_NONE; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeFastToCurrentMotorMask = 0; + mixerProfileAT.tailSitterManualTransition = false; + mixerProfileAT.tailSitterCaptureStableSince = 0; + isMixerTransitionMixing_requested = false; + manualProfileSwitchAutoTransitionActive = false; +} + +static bool updateTailSitterToMcCapture(void) +{ + const timeMs_t currentTimeMs = millis(); + + isMixerTransitionMixing_requested = true; + + if (!mixerTransitionTailSitterCapturePitchReached(attitude.values.pitch)) { + mixerProfileAT.tailSitterCaptureStableSince = 0; return false; } - if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) - { + + if (mixerProfileAT.tailSitterCaptureStableSince == 0) { + mixerProfileAT.tailSitterCaptureStableSince = currentTimeMs; + return false; + } + + if ((currentTimeMs - mixerProfileAT.tailSitterCaptureStableSince) < MIXER_TRANSITION_TAILSITTER_CAPTURE_CONFIRM_MS) { return false; } + completeTransition(); + return true; +} + +static bool isLegacyManualTransitionSessionActive(void) +{ + return manualTransitionSessionMode == MIXER_TRANSITION_MANUAL_SESSION_LEGACY; +} +#endif + +bool checkMixerATRequired(mixerProfileATRequest_e required_action) +{ +#ifdef USE_AUTO_TRANSITION + const bool requestAllowed = mixerTransitionIsRequestAllowed( + required_action, + STATE(AIRPLANE), + STATE(MULTIROTOR), + isModeActivationConditionPresent(BOXMIXERPROFILE), + currentMixerConfig.automated_switch, + platformTypeConfigured(PLATFORM_AIRPLANE), + missionTransitionToMultirotorTypeConfigured()); + + return requestAllowed && isTailSitterTransitionRequestSupported(required_action); +#else + // Legacy 512 KB targets keep the original automated-switch behaviour. if(currentMixerConfig.automated_switch){ if ((required_action == MIXERAT_REQUEST_RTH) && STATE(MULTIROTOR)) { @@ -143,14 +1026,122 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) } } return false; +#endif } bool mixerATUpdateState(mixerProfileATRequest_e required_action) { +#ifdef USE_AUTO_TRANSITION + //return true if mixerAT is done or not required + bool reprocessState; + do + { + reprocessState=false; + if (required_action == MIXERAT_REQUEST_ABORT) { + if (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE && mixerProfileAT.hotSwitchDone) { + // Once the target profile is active, abort cannot safely undo + // the hot-switch. Keep fading the old propulsion output toward + // the safe post-switch destination instead of freezing it. + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + updatePostSwitchFade(); + return true; + } + abortTransition(false, false); + return true; + } + switch (mixerProfileAT.phase) { + case MIXERAT_PHASE_IDLE: + //check if mixerAT is required + 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: + mixerProfileAT.request = required_action; + mixerProfileAT.direction = directionForRequest(required_action); + setMixerProfileAT(); + mixerProfileAT.tailSitterManualTransition = + isTailSitterProfilePairConfigured() && + isTailSitterTransitionRequestSupported(required_action); + startTransitionEntryServoHandoffFade(); + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; + reprocessState = true; + break; + case MIXERAT_PHASE_TRANSITIONING: + isMixerTransitionMixing_requested = true; + if (required_action != MIXERAT_REQUEST_NONE && required_action != mixerProfileAT.request) { + abortTransition(false, false); + return true; + } + + if (mixerATReadyForHotSwitch(mixerProfileAT.request)) { + const bool tailSitterToMcCapture = isTailSitterManualToMcCapture(); + if (!tailSitterToMcCapture) { + isMixerTransitionMixing_requested = false; + } + mixerProfileAT.progress = 1.0f; + updateTransitionScales(); + prepareServoHandoffFade(collectServoHandoffMask(nextMixerProfileIndex, true)); + preparePostSwitchFade(nextMixerProfileIndex); + if (!outputProfileHotSwitch(nextMixerProfileIndex)) { + abortTransition(false, false); + return true; + } + mixerProfileAT.hotSwitchDone = true; + startServoHandoffFade(); + if (tailSitterToMcCapture) { + mixerProfileAT.tailSitterCaptureStableSince = 0; + mixerProfileAT.phase = MIXERAT_PHASE_TAILSITTER_TO_MC_CAPTURE; + return false; + } + if (!startPostSwitchFade()) { + completeTransition(); + } + return true; + } else if ((mixerProfileAT.usedAirspeed || isTailSitterManualToMcCapture()) && + currentMixerConfig.vtolTransitionAirspeedTimeoutMs > 0 && + (millis() - mixerProfileAT.transitionStartTime) >= currentMixerConfig.vtolTransitionAirspeedTimeoutMs) { + abortTransition(true, false); + return true; + } + + updateTransitionScales(); + return false; + break; + case MIXERAT_PHASE_POST_SWITCH_FADE: + isMixerTransitionMixing_requested = false; + updatePostSwitchFade(); + return true; + case MIXERAT_PHASE_TAILSITTER_TO_MC_CAPTURE: + if (required_action == MIXERAT_REQUEST_MANUAL_TO_FW) { + // The MC profile is already active. Reverse through the normal + // MC->FW sequence rather than direct-switching its reference frame. + mixerProfileAT.request = required_action; + mixerProfileAT.direction = MIXERAT_DIRECTION_TO_FW; + mixerProfileAT.hotSwitchDone = false; + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITION_INITIALIZE; + reprocessState = true; + break; + } + + updateTailSitterToMcCapture(); + return true; + default: + break; + } + } + while (reprocessState); + return true; +#else //return true if mixerAT is done or not required bool reprocessState; do - { + { reprocessState=false; if (required_action==MIXERAT_REQUEST_ABORT){ isMixerTransitionMixing_requested = false; @@ -166,7 +1157,6 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) } break; case MIXERAT_PHASE_TRANSITION_INITIALIZE: - // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); setMixerProfileAT(); mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; reprocessState = true; @@ -188,6 +1178,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) } while (reprocessState); return true; +#endif } bool checkMixerProfileHotSwitchAvalibility(void) @@ -202,7 +1193,364 @@ bool checkMixerProfileHotSwitchAvalibility(void) void outputProfileUpdateTask(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - if(cliMode) return; + if (cliMode) { + return; + } + +#ifdef USE_AUTO_TRANSITION + 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 navigationFSMStateFlags_t navStateFlags = navGetCurrentStateFlags(); + const bool missionActive = (navStateFlags & NAV_AUTO_WP) != 0; + const bool autotransitionAlways = systemConfig()->vtolAutotransitionAlways && !missionActive; + const bool manualControllerConfigured = (currentMixerConfig.manualVtolTransitionController || autotransitionAlways) && !missionActive; + bool manualControllerEnabled = mixerTransitionManualControllerEnabled(manualControllerConfigured, manualTransitionSessionMode); + const bool mixerProfileModePresent = isModeActivationConditionPresent(BOXMIXERPROFILE); + const int requestedProfileIndex = IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1; + const bool keepCompletedAutoSession = mixerTransitionKeepCompletedAutoSession( + manualTransitionSessionMode, + transitionModeFallingEdge, + mixerProfileAT.hotSwitchDone, + currentMixerProfileIndex, + requestedProfileIndex); + const bool requestedMultirotorProfile = mixerProfileModePresent && + isMultirotorTypePlatform(mixerConfigByIndex(requestedProfileIndex)->platformType); + // If low-speed protection already moved the model back to MC, keep direct + // switching from forcing FW again until the pilot makes a new manual choice. + const bool fwToMcProtectionOwnsProfileSwitch = manualFwToMcProtectionLatched && + STATE(MULTIROTOR) && + !requestedMultirotorProfile; + const bool vtolProfilePairConfigured = + (isMultirotorTypePlatform(currentMixerConfig.platformType) && platformTypeConfigured(PLATFORM_AIRPLANE)) || + (currentMixerConfig.platformType == PLATFORM_AIRPLANE && missionTransitionToMultirotorTypeConfigured()); + const bool navigationOwnsProfileSwitch = mixerTransitionNavigationOwnsProfileSwitch( + ARMING_FLAG(ARMED), + vtolProfilePairConfigured, + (navStateFlags & NAV_AUTO_WP) != 0, + (navStateFlags & NAV_AUTO_RTH) != 0, + (navStateFlags & NAV_CTL_LAND) != 0, + (navStateFlags & NAV_MIXERAT) != 0); + const bool navigationProfileHandbackShouldHold = navigationProfileHandbackPending || + mixerTransitionNavigationHandbackShouldHoldProfile( + navigationProfileSwitchWasOwned, + navigationOwnsProfileSwitch, + mixerProfileModePresent, + mixerAT_inuse, + transitionModeActive, + currentMixerProfileIndex, + requestedProfileIndex); + const bool manualSwitchInputAllowed = mixerTransitionManualInputAllowed( + navigationOwnsProfileSwitch, + navigationProfileHandbackShouldHold); + const bool completedAutoSessionEndpointConfirmed = mixerTransitionCompletedAutoSessionEndpointConfirmed( + manualTransitionSessionMode, + mixerProfileAT.hotSwitchDone, + transitionModeActive, + currentMixerProfileIndex, + requestedProfileIndex); + + manualTransitionSessionMode = mixerTransitionUpdateManualSessionMode( + manualTransitionSessionMode, + transitionModeRisingEdge && manualSwitchInputAllowed, + transitionModeFallingEdge && manualSwitchInputAllowed && !keepCompletedAutoSession, + manualControllerConfigured, + !manualSwitchInputAllowed); + + const bool directSwitchEndpointOwnsServoOutput = manualSwitchInputAllowed && + mixerTransitionDirectSwitchEndpointOwnsServoOutput( + manualTransitionSessionMode, + mixerAT_inuse, + transitionModeActive, + mixerProfileAT.hotSwitchDone, + mixerProfileAT.aborted, + mixerProfileAT.directSwitchAbort, + currentMixerProfileIndex, + requestedProfileIndex); + + if (transitionModeRisingEdge && manualSwitchInputAllowed) { + manualFwToMcProtectionLatched = false; + } + + if (directSwitchEndpointOwnsServoOutput) { + clearServoHandoffFade(); + } else { + updateServoHandoffFadeExpiration(); + } + + if (completedAutoSessionEndpointConfirmed) { + manualTransitionSessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + clearServoHandoffFade(); + } + + if (manualSwitchInputAllowed && + (requestedMultirotorProfile || (!mixerAT_inuse && !STATE(MULTIROTOR)))) { + manualFwToMcProtectionLatched = false; + } + + const bool failsafeShouldAbortTransition = FLIGHT_MODE(FAILSAFE_MODE) && + mixerTransitionShouldAbortForFailsafe( + mixerProfileAT.request, + mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE, + mixerProfileAT.hotSwitchDone); + + if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || failsafeShouldAbortTransition || areSensorsCalibrating())) { + abortTransition(false, false); + manualTransitionSessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + manualFwToMcProtectionLatched = false; + mixerAT_inuse = false; + } + + if (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE) { + mixerATUpdateState(MIXERAT_REQUEST_NONE); + mixerAT_inuse = mixerATIsActive(); + } + + if (!ARMING_FLAG(ARMED)) { + navigationProfileSwitchWasOwned = false; + navigationProfileHandbackPending = false; + } else if (mixerTransitionNavigationHandbackShouldClear( + navigationOwnsProfileSwitch, + transitionModeActive, + currentMixerProfileIndex, + requestedProfileIndex)) { + navigationProfileHandbackPending = false; + } else if (navigationProfileHandbackShouldHold) { + navigationProfileHandbackPending = true; + } + navigationProfileSwitchWasOwned = navigationOwnsProfileSwitch; + + const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; + const bool completedAutoSessionOwnsProfileSwitch = mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + manualTransitionSessionMode, + mixerProfileAT.hotSwitchDone, + currentMixerProfileIndex, + requestedProfileIndex); + const bool profileSwitchShouldStartAutoTransition = mixerTransitionProfileSwitchShouldStartAutoTransition( + autotransitionAlways, + ARMING_FLAG(ARMED), + manualTransitionAllowed, + mixerProfileModePresent, + vtolProfilePairConfigured, + mixerAT_inuse, + navigationOwnsProfileSwitch, + navigationProfileHandbackPending, + completedAutoSessionOwnsProfileSwitch, + fwToMcProtectionOwnsProfileSwitch, + currentMixerProfileIndex, + requestedProfileIndex); + const bool profileSwitchDirectSwitchAllowed = mixerTransitionProfileSwitchDirectSwitchAllowed( + autotransitionAlways, + ARMING_FLAG(ARMED), + vtolProfilePairConfigured); + + if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) + { + if (mixerProfileModePresent && + (!transitionControllerOwnsProfileSwitch || profileSwitchShouldStartAutoTransition) && + !completedAutoSessionOwnsProfileSwitch && + !navigationOwnsProfileSwitch && + !navigationProfileHandbackPending && + !fwToMcProtectionOwnsProfileSwitch) { + if (requestedProfileIndex != currentMixerProfileIndex) { + if (profileSwitchShouldStartAutoTransition) { + const mixerProfileATRequest_e profileSwitchRequest = STATE(MULTIROTOR) ? + MIXERAT_REQUEST_MANUAL_TO_FW : + (STATE(AIRPLANE) ? MIXERAT_REQUEST_MANUAL_TO_MC : MIXERAT_REQUEST_NONE); + + if (profileSwitchRequest != MIXERAT_REQUEST_NONE && checkMixerATRequired(profileSwitchRequest)) { + manualProfileSwitchAutoTransitionActive = true; + manualTransitionSessionMode = MIXER_TRANSITION_MANUAL_SESSION_AUTO; + manualTransitionReadyForEdge = false; + mixerATUpdateState(profileSwitchRequest); + mixerAT_inuse = mixerATIsActive(); + } + } else { + if (profileSwitchDirectSwitchAllowed) { + outputProfileDirectSwitch(requestedProfileIndex, vtolProfilePairConfigured); + } + } + } + } + } + + // Recompute after a potential direct profile hot-switch because this flag is per-mixer-profile. + manualControllerEnabled = mixerTransitionManualControllerEnabled( + (currentMixerConfig.manualVtolTransitionController || autotransitionAlways) && !missionActive, + manualTransitionSessionMode); + + 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 (navigationOwnsProfileSwitch && !mixerAT_inuse) { + // A mission/RTH/landing transition owns this state. Do not leave a + // manual request behind when its RC switch briefly crosses middle. + isMixerTransitionMixing_requested = false; + } else if (mixerTransitionManualMixingRequestMayUpdate( + FLIGHT_MODE(FAILSAFE_MODE), + mixerAT_inuse, + navigationOwnsProfileSwitch, + navigationProfileHandbackPending)) { + isMixerTransitionMixing_requested = transitionModeActive; + if (isMixerTransitionMixing_requested) { + setLegacyTransitionScales(); + } + } + manualTransitionReadyForEdge = true; + } else { + if (!transitionModeActive) { + manualTransitionReadyForEdge = true; + if (!mixerAT_inuse) { + isMixerTransitionMixing_requested = false; + } + } else if (mixerTransitionShouldClearCompletedAutoMixingRequest( + transitionModeActive, + transitionModeRisingEdge, + mixerAT_inuse, + mixerProfileAT.hotSwitchDone)) { + isMixerTransitionMixing_requested = false; + } else if (transitionModeRisingEdge && manualSwitchInputAllowed && 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(); + } + + const bool transitionSwitchAbort = !manualProfileSwitchAutoTransitionActive && !transitionModeActive; + const bool profileSwitchAbort = mixerTransitionProfileSwitchShouldAbortToCurrentProfile( + manualProfileSwitchAutoTransitionActive, + mixerAT_inuse, + mixerProfileAT.hotSwitchDone, + currentMixerProfileIndex, + requestedProfileIndex); + + if ((transitionSwitchAbort || profileSwitchAbort) && + mixerAT_inuse && + !mixerProfileAT.hotSwitchDone && + (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { + abortTransition(false, true); + mixerAT_inuse = false; + if (!FLIGHT_MODE(FAILSAFE_MODE) && + mixerProfileModePresent && + !navigationOwnsProfileSwitch && + requestedProfileIndex != currentMixerProfileIndex) { + outputProfileDirectSwitch(requestedProfileIndex, true); + } + } + + mixerProfileATRequest_e activeManualTransitionRequest = mixerProfileAT.request; + if (mixerTransitionTailSitterCaptureShouldReverse( + mixerProfileAT.tailSitterManualTransition, + mixerProfileAT.phase == MIXERAT_PHASE_TAILSITTER_TO_MC_CAPTURE, + transitionModeActive, + STATE(MULTIROTOR), + currentMixerProfileIndex, + requestedProfileIndex)) { + activeManualTransitionRequest = MIXERAT_REQUEST_MANUAL_TO_FW; + } + + if (mixerAT_inuse && + (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { + mixerATUpdateState(activeManualTransitionRequest); + mixerAT_inuse = mixerATIsActive(); + } + } + + updateMixerATSwitchReminder(transitionModeActive, requestedProfileIndex); + + manualTransitionModeWasActive = transitionModeActive; + + isMixerTransitionMixing = isMixerTransitionMixing_requested && + ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); + + 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) | + (manualTransitionSessionMode != MIXER_TRANSITION_MANUAL_SESSION_NONE ? 1U << 20 : 0U) | + (isLegacyManualTransitionSessionActive() ? 1U << 30 : 0U) | + (mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE ? 1U << 29 : 0U); + + uint8_t targetInputMode = 0U; + if (isMixerTransitionMixing && mixerATIsActive()) { + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + targetInputMode = FLIGHT_MODE(MANUAL_MODE) ? 1U : 2U; + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + targetInputMode = 3U; + } + } + const uint16_t progressScaled = lrintf(constrainf(mixerProfileAT.progress, 0.0f, 1.0f) * 1000.0f); + const uint16_t handoffScaled = lrintf(constrainf(mixerProfileAT.handoffScalingProgress, 0.0f, 1.0f) * 1000.0f); + const uint16_t motorRampScaled = lrintf(constrainf(mixerProfileAT.motorRampProgress, 0.0f, 1.0f) * 1000.0f); + const uint16_t postFadeScaled = lrintf(constrainf(mixerProfileAT.postSwitchFadeProgress, 0.0f, 1.0f) * 1000.0f); + const uint16_t pusherScaled = lrintf(constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f) * 1000.0f); + const uint16_t liftScaled = lrintf(constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) * 1000.0f); + const uint16_t mcAuthorityScaled = lrintf(constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f) * 1000.0f); + const uint16_t fwAuthorityScaled = lrintf(constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f) * 1000.0f); + const uint32_t packedProgress = + ((uint32_t)MIN(handoffScaled, 1000) & 0x3FFU) | + (((uint32_t)MIN(motorRampScaled, 1000) & 0x3FFU) << 10) | + (((uint32_t)MIN(postFadeScaled, 1000) & 0x3FFU) << 20); + + transitionDebugFlags |= (((uint32_t)currentMixerConfig.platformType & 0xFU) << 21); + transitionDebugFlags |= (((uint32_t)pidIndexGetType(PID_ROLL) & 0x3U) << 25); + transitionDebugFlags |= (((uint32_t)targetInputMode & 0x3U) << 27); + + // VTOL transition debug channels (DEBUG_VTOL_TRANSITION): + // [0] phase + // [1] request | (direction << 8) | (waitReason << 16) + // [2] packed transition flags | active platform type | active PID type | target input mode + // [3] raw transition progress x1000 + // [4] pusherScale x1000 + // [5] liftScale x1000 + // [6] active: mcAuthorityScale x1000 | (fwAuthorityScale x1000 << 16) + // idle: servo debug slot 0, see servoMixerGetVtolTransitionDebug() + // [7] active: handoffProgress 10-bit | motorRampProgress 10-bit | postSwitchFadeProgress 10-bit + // idle: servo debug slot 1, see servoMixerGetVtolTransitionDebug() + const bool useIdleServoDebug = !mixerATIsActive() && mixerProfileAT.phase == MIXERAT_PHASE_IDLE; + DEBUG_SET(DEBUG_VTOL_TRANSITION, 0, mixerProfileAT.phase); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, (int32_t)(((uint32_t)mixerProfileAT.request & 0xFFU) | (((uint32_t)mixerProfileAT.direction & 0xFFU) << 8) | (((uint32_t)mixerProfileAT.waitReason & 0xFFU) << 16))); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 2, (int32_t)transitionDebugFlags); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 3, progressScaled); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 4, pusherScaled); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 5, liftScaled); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 6, useIdleServoDebug ? servoMixerGetVtolTransitionDebug(0) : (int32_t)((uint32_t)mcAuthorityScaled | ((uint32_t)fwAuthorityScaled << 16))); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, useIdleServoDebug ? servoMixerGetVtolTransitionDebug(1) : (int32_t)packedProgress); + + if (!isMixerTransitionMixing && !mixerATIsActive()) { + mixerTransitionServoInput = 0; + resetTransitionScales(); + } +#else bool mixerAT_inuse = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; // transition mode input for servo mix and motor mix if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) @@ -213,6 +1561,213 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); } isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); +#endif +} + +bool NOINLINE mixerATIsActive(void) +{ + return mixerProfileAT.phase != MIXERAT_PHASE_IDLE; +} + +#ifdef USE_AUTO_TRANSITION +bool mixerATGetOsdStatus(mixerProfileATOsdStatus_t *status) +{ + if (!status) { + return false; + } + + const bool active = mixerATIsActive(); + const bool eventActive = mixerATOsdEvent != MIXERAT_OSD_EVENT_NONE && cmp32(millis(), mixerATOsdEventUntil) < 0; + const bool switchReminderActive = mixerATOsdSwitchReminderDirection != MIXERAT_DIRECTION_NONE; + + if (!active && !eventActive && !switchReminderActive) { + return false; + } + + status->active = active; + status->phase = mixerProfileAT.phase; + status->direction = mixerProfileAT.direction; + status->request = mixerProfileAT.request; + status->event = eventActive ? mixerATOsdEvent : MIXERAT_OSD_EVENT_NONE; + status->waitReason = active ? mixerProfileAT.waitReason : MIXERAT_WAIT_REASON_NONE; + status->switchReminderDirection = switchReminderActive ? mixerATOsdSwitchReminderDirection : MIXERAT_DIRECTION_NONE; + + if (!eventActive) { + mixerATOsdEvent = MIXERAT_OSD_EVENT_NONE; + mixerATOsdEventUntil = 0; + } + + return true; +} +#endif + +bool mixerATWasAborted(void) +{ +#ifdef USE_AUTO_TRANSITION + return mixerProfileAT.aborted; +#else + return false; +#endif +} + +bool mixerATWasAbortedByAirspeedTimeout(void) +{ +#ifdef USE_AUTO_TRANSITION + return mixerProfileAT.abortedByAirspeedTimeout; +#else + return false; +#endif +} + +float NOINLINE mixerATGetPusherScale(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif +} + +float NOINLINE mixerATGetLiftScale(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif +} + +float NOINLINE mixerATGetMcAuthorityScale(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif +} + +float NOINLINE mixerATGetFwAuthorityScale(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif +} + +float NOINLINE mixerATGetBlendToFw(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.blendToFw, 0.0f, 1.0f); +#else + return 1.0f; +#endif +} + +int16_t NOINLINE mixerATGetTransitionServoInput(void) +{ +#ifdef USE_AUTO_TRANSITION + const bool postSwitchFadeToFwActive = + mixerProfileAT.phase == MIXERAT_PHASE_POST_SWITCH_FADE && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW; + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + const float servoBlendToFw = mixerTransitionComputeServoBlendToFw( + isLegacyManualTransitionSessionActive(), + isMixerTransitionMixing, + mixerATIsActive(), + postSwitchFadeToFwActive, + currentMixerConfig.vtolTransitionDynamicMixer, + mixerProfileAT.direction, + currentMixerConfig.vtolTransitionScaleRampTimeMs, + elapsedMs); + + mixerTransitionServoInput = mixerTransitionUpdateServoInput( + mixerTransitionServoInput, + isLegacyManualTransitionSessionActive(), + isMixerTransitionMixing, + mixerATIsActive(), + postSwitchFadeToFwActive, + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW, + servoBlendToFw); + + return mixerTransitionServoInput; +#else + return isMixerTransitionMixing ? 500 : 0; +#endif +} + +#ifdef USE_AUTO_TRANSITION +bool mixerATGetServoHandoffOutput(uint8_t servoIndex, int16_t currentOutput, int16_t *output) +{ + if (servoIndex >= MAX_SUPPORTED_SERVOS || + mixerProfileAT.servoHandoffMask == 0 || + (mixerProfileAT.servoHandoffMask & (1U << servoIndex)) == 0) { + return false; + } + + const uint32_t elapsedMs = millis() - mixerProfileAT.servoHandoffStartTime; + if (mixerTransitionServoHandoffExpired(elapsedMs, mixerProfileAT.servoHandoffDurationMs)) { + clearServoHandoffFade(); + return false; + } + + const float progress = mixerTransitionServoHandoffProgress(elapsedMs, mixerProfileAT.servoHandoffDurationMs); + + *output = mixerTransitionBlendCapturedServoOutput( + mixerProfileAT.servoHandoffOutput[servoIndex], + currentOutput, + progress); + + return true; +} + +bool NOINLINE mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output) +{ + if (mixerProfileAT.phase != MIXERAT_PHASE_POST_SWITCH_FADE || + motorIndex >= MAX_SUPPORTED_MOTORS || + (mixerProfileAT.postSwitchFadeMotorMask & (1U << motorIndex)) == 0) { + return false; + } + + const int16_t capturedOutput = mixerProfileAT.postSwitchFadeMotorOutput[motorIndex]; + const bool fadeToCurrentOutput = (mixerProfileAT.postSwitchFadeToCurrentMotorMask & (1U << motorIndex)) != 0; + const int16_t targetOutput = fadeToCurrentOutput ? currentOutput : idleOutput; + float progress = mixerProfileAT.postSwitchFadeProgress; + + if (mixerProfileAT.postSwitchFadeFastToCurrentMotorMask & (1U << motorIndex)) { + const uint16_t durationMs = mixerProfileAT.postSwitchFadeDurationMs < MIXER_TRANSITION_MC_LIFT_OUTPUT_HANDOFF_MAX_MS ? + mixerProfileAT.postSwitchFadeDurationMs : MIXER_TRANSITION_MC_LIFT_OUTPUT_HANDOFF_MAX_MS; + progress = durationMs == 0 ? 1.0f : constrainf( + (float)(millis() - mixerProfileAT.postSwitchFadeStartTime) / (float)durationMs, + 0.0f, + 1.0f); + } + + *output = mixerTransitionBlendCapturedMotorOutput( + capturedOutput, + targetOutput, + progress); + return true; +} + +float mixerATGetPostSwitchFadeProgress(void) +{ + return constrainf(mixerProfileAT.postSwitchFadeProgress, 0.0f, 1.0f); +} +#endif + +bool isMixerProfile2ModeReportedActive(void) +{ +#if (MAX_MIXER_PROFILE_COUNT > 1) + return mixerTransitionProfile2ShouldReportActive(currentMixerProfileIndex); +#else + return false; +#endif +} + +bool isMixerTransitionModeReportedActive(void) +{ + return mixerTransitionModeShouldReportActive(mixerATIsActive(), isMixerTransitionMixing); } // switch mixerprofile without reboot diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 715732d0685..c82e35131b9 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,6 +18,12 @@ typedef struct mixerConfig_s { bool controlProfileLinking; bool automated_switch; int16_t switchTransitionTimer; +#ifdef USE_AUTO_TRANSITION + bool vtolTransitionDynamicMixer; + bool manualVtolTransitionController; + uint16_t vtolTransitionAirspeedTimeoutMs; + uint16_t vtolTransitionScaleRampTimeMs; +#endif bool tailsitterOrientationOffset; int16_t transition_PID_mmix_multiplier_roll; int16_t transition_PID_mmix_multiplier_pitch; @@ -34,27 +40,159 @@ typedef enum { MIXERAT_REQUEST_NONE, //no request, stats checking only MIXERAT_REQUEST_RTH, MIXERAT_REQUEST_LAND, +#ifdef USE_AUTO_TRANSITION + MIXERAT_REQUEST_MISSION_TO_FW, + MIXERAT_REQUEST_MISSION_TO_MC, + MIXERAT_REQUEST_MANUAL_TO_FW, + MIXERAT_REQUEST_MANUAL_TO_MC, + MIXERAT_REQUEST_FW_TO_MC_PROTECTION, +#endif MIXERAT_REQUEST_ABORT, } mixerProfileATRequest_e; +#ifdef USE_AUTO_TRANSITION +#define MIXER_TRANSITION_AIRSPEED_CONFIRM_MS 300 +#define MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_MS 100 +#define MIXER_TRANSITION_MC_LIFT_OUTPUT_HANDOFF_MAX_MS MIXER_TRANSITION_AIRSPEED_CONFIRM_MS +#define MIXER_TRANSITION_TAILSITTER_CAPTURE_PITCH_DECIDEGREES 450 +#define MIXER_TRANSITION_TAILSITTER_CAPTURE_PITCH_TOLERANCE_DECIDEGREES 100 +#define MIXER_TRANSITION_TAILSITTER_CAPTURE_CONFIRM_MS 300 +#define MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT \ + (MIXER_TRANSITION_AIRSPEED_CONFIRM_MS / MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_MS + 1) + +typedef struct mixerTransitionAirspeedProgressFilter_s { + bool active; + bool windowMature; + uint8_t bucketCount; + uint8_t nextBucket; + timeMs_t windowStartTime; + timeMs_t acceptedProgressUpdateTime; + bool acceptedProgressUpdateValid; + float recentMinProgress; + float recentMaxProgress; + timeMs_t bucketStartTime[MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT]; + timeMs_t bucketEndTime[MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT]; + float bucketMinProgress[MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT]; + float bucketMaxProgress[MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT]; +} mixerTransitionAirspeedProgressFilter_t; + +typedef enum { + MIXERAT_DIRECTION_NONE = 0, + MIXERAT_DIRECTION_TO_FW, + MIXERAT_DIRECTION_TO_MC, +} mixerProfileATDirection_e; + +typedef enum { + MIXERAT_OSD_EVENT_NONE = 0, + MIXERAT_OSD_EVENT_DONE, + MIXERAT_OSD_EVENT_ABORTED, + MIXERAT_OSD_EVENT_ABORTED_IN_FW, + MIXERAT_OSD_EVENT_ABORTED_IN_MC, + MIXERAT_OSD_EVENT_AIRSPEED_TIMEOUT, +} mixerProfileATOsdEvent_e; + +typedef enum { + MIXERAT_WAIT_REASON_NONE = 0, + MIXERAT_WAIT_REASON_MC_SPEED, + MIXERAT_WAIT_REASON_NO_SPEED, + MIXERAT_WAIT_REASON_MC_SPEED_HIGH, +} mixerProfileATWaitReason_e; +#endif + //mixerProfile Automated Transition PHASE typedef enum { MIXERAT_PHASE_IDLE, MIXERAT_PHASE_TRANSITION_INITIALIZE, MIXERAT_PHASE_TRANSITIONING, +#ifdef USE_AUTO_TRANSITION + MIXERAT_PHASE_POST_SWITCH_FADE, + MIXERAT_PHASE_TAILSITTER_TO_MC_CAPTURE, +#endif +#ifndef USE_AUTO_TRANSITION MIXERAT_PHASE_DONE, +#endif } mixerProfileATState_e; typedef struct mixerProfileAT_s { mixerProfileATState_e phase; +#ifdef USE_AUTO_TRANSITION + mixerProfileATDirection_e direction; + mixerProfileATRequest_e request; + bool aborted; + bool directSwitchAbort; + bool abortedByAirspeedTimeout; + bool hotSwitchDone; + bool usedAirspeed; + bool tailSitterManualTransition; + mixerProfileATWaitReason_e waitReason; + bool transitionStartAirspeedCaptured; + float progress; + float handoffScalingProgress; + mixerTransitionAirspeedProgressFilter_t airspeedProgressFilter; + float motorRampProgress; + float transitionStartAirspeedCmS; + float blendToFw; + float pusherScale; + float liftScale; + float mcAuthorityScale; + float fwAuthorityScale; + float postSwitchFadeProgress; + float postSwitchFadeInitialScale; + uint16_t postSwitchFadeMotorMask; + uint16_t postSwitchFadeToCurrentMotorMask; + uint16_t postSwitchFadeFastToCurrentMotorMask; + uint16_t postSwitchFadeDurationMs; + uint16_t postSwitchFadeMotorOutput[MAX_SUPPORTED_MOTORS]; + timeMs_t postSwitchFadeStartTime; + uint32_t servoHandoffMask; + uint16_t servoHandoffDurationMs; + uint16_t servoHandoffHoldDurationMs; + int16_t servoHandoffOutput[MAX_SUPPORTED_SERVOS]; + timeMs_t servoHandoffStartTime; + timeMs_t servoHandoffHoldStartTime; + timeMs_t tailSitterCaptureStableSince; + timeMs_t transitionStartTime; +#else bool transitionInputMixing; timeMs_t transitionStartTime; timeMs_t transitionStabEndTime; timeMs_t transitionTransEndTime; +#endif } mixerProfileAT_t; + +#ifdef USE_AUTO_TRANSITION +typedef struct mixerProfileATOsdStatus_s { + bool active; + mixerProfileATState_e phase; + mixerProfileATDirection_e direction; + mixerProfileATRequest_e request; + mixerProfileATOsdEvent_e event; + mixerProfileATWaitReason_e waitReason; + mixerProfileATDirection_e switchReminderDirection; +} mixerProfileATOsdStatus_t; +#endif + 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); +int16_t mixerATGetTransitionServoInput(void); +#ifdef USE_AUTO_TRANSITION +bool mixerATGetOsdStatus(mixerProfileATOsdStatus_t *status); +bool mixerATGetPostSwitchFadeMotorOutput(uint8_t motorIndex, int16_t idleOutput, int16_t currentOutput, int16_t *output); +float mixerATGetPostSwitchFadeProgress(void); +bool mixerATGetServoHandoffOutput(uint8_t servoIndex, int16_t currentOutput, int16_t *output); +bool outputProfileDirectSwitchImmediate(int profile_index); +#endif +bool isMixerProfile2ModeReportedActive(void); +bool isMixerTransitionModeReportedActive(void); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; @@ -81,4 +219,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/mixer_transition_logic.h b/src/main/flight/mixer_transition_logic.h new file mode 100644 index 00000000000..8c828483a0d --- /dev/null +++ b/src/main/flight/mixer_transition_logic.h @@ -0,0 +1,1014 @@ +#pragma once + +#include +#include + +#include "flight/mixer_profile.h" + +typedef enum { + MIXER_TRANSITION_MANUAL_SESSION_NONE = 0, + MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + MIXER_TRANSITION_MANUAL_SESSION_AUTO, +} mixerTransitionManualSessionMode_e; + +static inline bool mixerTransitionProfile2ShouldReportActive(int currentProfileIndex) +{ + return currentProfileIndex > 0; +} + +static inline bool mixerTransitionModeShouldReportActive( + bool autoTransitionActive, + bool transitionMixingActive) +{ + // Active Modes reports the effective output state, not the position of a + // switch that may currently be ignored by navigation. + return autoTransitionActive || transitionMixingActive; +} + +#ifdef USE_AUTO_TRANSITION +typedef struct { + bool readyForHotSwitch; + bool usedAirspeed; + bool transitionStartAirspeedCaptured; + float transitionStartAirspeedCmS; + float progress; +} mixerTransitionHotSwitchProgress_t; + +typedef struct { + float pusherScale; + float liftScale; + float mcAuthorityScale; + float fwAuthorityScale; + float blendToFw; +} mixerTransitionScaleState_t; + +typedef struct { + uint16_t motorMask; + uint16_t toCurrentMotorMask; + uint16_t fastToCurrentMotorMask; +} mixerTransitionPostSwitchFadeMask_t; +#endif + +static inline mixerTransitionManualSessionMode_e mixerTransitionUpdateManualSessionMode( + mixerTransitionManualSessionMode_e currentMode, + bool transitionModeRisingEdge, + bool transitionModeFallingEdge, + bool manualControllerConfigured, + bool clearSession) +{ + if (clearSession || transitionModeFallingEdge) { + return MIXER_TRANSITION_MANUAL_SESSION_NONE; + } + + if (transitionModeRisingEdge) { + return manualControllerConfigured ? + MIXER_TRANSITION_MANUAL_SESSION_AUTO : + MIXER_TRANSITION_MANUAL_SESSION_LEGACY; + } + + return currentMode; +} + +static inline float mixerTransitionClamp(float value, float low, float high) +{ + if (value < low) { + return low; + } + + if (value > high) { + return high; + } + + return value; +} + +static inline bool mixerTransitionManualControllerEnabled( + bool manualControllerConfigured, + mixerTransitionManualSessionMode_e sessionMode) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO || + (sessionMode != MIXER_TRANSITION_MANUAL_SESSION_LEGACY && manualControllerConfigured); +} + +#ifdef USE_AUTO_TRANSITION +static inline bool mixerTransitionIsTailSitterProfilePair( + bool currentProfileIsAirplane, + bool currentProfileIsMultirotor, + bool currentProfileIsTailSitter, + bool targetProfileIsAirplane, + bool targetProfileIsMultirotor, + bool targetProfileIsTailSitter) +{ + return (currentProfileIsMultirotor && currentProfileIsTailSitter && + targetProfileIsAirplane && !targetProfileIsTailSitter) || + (currentProfileIsAirplane && !currentProfileIsTailSitter && + targetProfileIsMultirotor && targetProfileIsTailSitter); +} + +static inline bool mixerTransitionTailSitterRequestIsSupported( + mixerProfileATRequest_e requiredAction, + bool tailSitterProfilePair, + bool currentDynamicMixerEnabled, + bool targetDynamicMixerEnabled) +{ + if (!tailSitterProfilePair) { + return true; + } + + // The first tailsitter implementation deliberately keeps automatic + // navigation and dynamic mixer paths out of the unvalidated airframe. + return (requiredAction == MIXERAT_REQUEST_MANUAL_TO_FW || + requiredAction == MIXERAT_REQUEST_MANUAL_TO_MC) && + !currentDynamicMixerEnabled && + !targetDynamicMixerEnabled; +} + +static inline bool mixerTransitionTailSitterNeedsMcCapture( + mixerProfileATRequest_e requiredAction, + bool tailSitterProfilePair) +{ + return tailSitterProfilePair && requiredAction == MIXERAT_REQUEST_MANUAL_TO_MC; +} + +static inline bool mixerTransitionTailSitterCapturePitchReached(int16_t pitchDeciDegrees) +{ + return pitchDeciDegrees >= + MIXER_TRANSITION_TAILSITTER_CAPTURE_PITCH_DECIDEGREES - MIXER_TRANSITION_TAILSITTER_CAPTURE_PITCH_TOLERANCE_DECIDEGREES && + pitchDeciDegrees <= + MIXER_TRANSITION_TAILSITTER_CAPTURE_PITCH_DECIDEGREES + MIXER_TRANSITION_TAILSITTER_CAPTURE_PITCH_TOLERANCE_DECIDEGREES; +} + +static inline bool mixerTransitionTailSitterCaptureShouldReverse( + bool tailSitterManualTransition, + bool captureActive, + bool transitionModeActive, + bool currentProfileIsMultirotor, + int currentProfileIndex, + int requestedProfileIndex) +{ + // After the FW->MC hot-switch the output frame has already changed. A + // pilot request for FW must therefore run the known MC->FW path, never a + // direct profile switch back through the changing reference frame. + return tailSitterManualTransition && + captureActive && + !transitionModeActive && + currentProfileIsMultirotor && + currentProfileIndex != requestedProfileIndex; +} + +static inline bool mixerTransitionKeepCompletedAutoSession( + mixerTransitionManualSessionMode_e sessionMode, + bool transitionModeFallingEdge, + bool hotSwitchDone, + int currentProfileIndex, + int requestedProfileIndex) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && + !transitionModeFallingEdge && + hotSwitchDone && + currentProfileIndex != requestedProfileIndex; +} + +static inline bool mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + mixerTransitionManualSessionMode_e sessionMode, + bool hotSwitchDone, + int currentProfileIndex, + int requestedProfileIndex) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && + hotSwitchDone && + currentProfileIndex != requestedProfileIndex; +} + +static inline bool mixerTransitionCompletedAutoSessionEndpointConfirmed( + mixerTransitionManualSessionMode_e sessionMode, + bool hotSwitchDone, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && + hotSwitchDone && + !transitionModeActive && + currentProfileIndex == requestedProfileIndex; +} + +static inline bool mixerTransitionDirectSwitchEndpointOwnsServoOutput( + mixerTransitionManualSessionMode_e sessionMode, + bool autoTransitionActive, + bool transitionModeActive, + bool hotSwitchDone, + bool transitionAborted, + bool directSwitchAbort, + int currentProfileIndex, + int requestedProfileIndex) +{ + // A direct profile selection has no transition-owned servo phase. Any + // leftover handoff state would keep the target profile's surfaces slow. + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_NONE && + !autoTransitionActive && + !transitionModeActive && + !hotSwitchDone && + (!transitionAborted || directSwitchAbort) && + currentProfileIndex == requestedProfileIndex; +} + +static inline mixerProfileATDirection_e mixerTransitionManualSwitchReminderDirection( + mixerTransitionManualSessionMode_e sessionMode, + bool autoTransitionActive, + bool hotSwitchDone, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex, + bool currentProfileIsMultirotor) +{ + if (sessionMode != MIXER_TRANSITION_MANUAL_SESSION_AUTO || + autoTransitionActive || + !hotSwitchDone) { + return MIXERAT_DIRECTION_NONE; + } + + if (!transitionModeActive && currentProfileIndex == requestedProfileIndex) { + return MIXERAT_DIRECTION_NONE; + } + + return currentProfileIsMultirotor ? MIXERAT_DIRECTION_TO_MC : MIXERAT_DIRECTION_TO_FW; +} + +static inline bool mixerTransitionNavigationHandbackShouldHoldProfile( + bool navigationOwnedProfileSwitchPreviousUpdate, + bool navigationOwnsProfileSwitch, + bool mixerProfileModePresent, + bool autoTransitionActive, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex) +{ + return navigationOwnedProfileSwitchPreviousUpdate && + !navigationOwnsProfileSwitch && + mixerProfileModePresent && + !autoTransitionActive && + (transitionModeActive || currentProfileIndex != requestedProfileIndex); +} + +static inline bool mixerTransitionNavigationHandbackShouldClear( + bool navigationOwnsProfileSwitch, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex) +{ + return navigationOwnsProfileSwitch || + (!transitionModeActive && currentProfileIndex == requestedProfileIndex); +} + +static inline bool mixerTransitionShouldClearCompletedAutoMixingRequest( + bool transitionModeActive, + bool transitionModeRisingEdge, + bool autoTransitionActive, + bool hotSwitchDone) +{ + return transitionModeActive && !transitionModeRisingEdge && !autoTransitionActive && hotSwitchDone; +} + +static inline bool mixerTransitionNavigationOwnsProfileSwitch( + bool armed, + bool vtolProfilePairConfigured, + bool navWaypointActive, + bool navRthActive, + bool navLandingActive, + bool navMixerAtActive) +{ + return armed && + vtolProfilePairConfigured && + (navWaypointActive || navRthActive || navLandingActive || navMixerAtActive); +} + +static inline bool mixerTransitionManualInputAllowed( + bool navigationOwnsProfileSwitch, + bool navigationProfileHandbackPending) +{ + // The RC modes remain visible to the pilot, but navigation-owned VTOL + // transitions must not inherit a transient middle switch position. Keep + // doing this while waiting for the pilot to match the profile selected by + // a completed mission/RTH transition. + return !navigationOwnsProfileSwitch && !navigationProfileHandbackPending; +} + +static inline bool mixerTransitionManualMixingRequestMayUpdate( + bool failsafeActive, + bool autoTransitionActive, + bool navigationOwnsProfileSwitch, + bool navigationProfileHandbackPending) +{ + return !failsafeActive && + !autoTransitionActive && + mixerTransitionManualInputAllowed(navigationOwnsProfileSwitch, navigationProfileHandbackPending); +} + +static inline bool mixerTransitionProfileSwitchShouldStartAutoTransition( + bool autoTransitionAlways, + bool armed, + bool manualTransitionAllowed, + bool mixerProfileModePresent, + bool vtolProfilePairConfigured, + bool autoTransitionActive, + bool navigationOwnsProfileSwitch, + bool navigationProfileHandbackPending, + bool completedAutoSessionOwnsProfileSwitch, + bool fwToMcProtectionOwnsProfileSwitch, + int currentProfileIndex, + int requestedProfileIndex) +{ + return autoTransitionAlways && + armed && + manualTransitionAllowed && + mixerProfileModePresent && + vtolProfilePairConfigured && + !autoTransitionActive && + !navigationOwnsProfileSwitch && + !navigationProfileHandbackPending && + !completedAutoSessionOwnsProfileSwitch && + !fwToMcProtectionOwnsProfileSwitch && + currentProfileIndex != requestedProfileIndex; +} + +static inline bool mixerTransitionProfileSwitchDirectSwitchAllowed( + bool autoTransitionAlways, + bool armed, + bool vtolProfilePairConfigured) +{ + return !autoTransitionAlways || !armed || !vtolProfilePairConfigured; +} + +static inline bool mixerTransitionProfileSwitchShouldAbortToCurrentProfile( + bool profileSwitchAutoTransitionActive, + bool autoTransitionActive, + bool hotSwitchDone, + int currentProfileIndex, + int requestedProfileIndex) +{ + return profileSwitchAutoTransitionActive && + autoTransitionActive && + !hotSwitchDone && + currentProfileIndex == requestedProfileIndex; +} +#endif + +static inline bool mixerTransitionIsRequestAllowed( + mixerProfileATRequest_e requiredAction, + bool stateAirplane, + bool stateMultirotor, + bool mixerProfileModePresent, + bool automatedSwitch, + bool targetProfileIsAirplane, + bool targetProfileIsMultirotor) +{ + if ((!stateAirplane && !stateMultirotor) || !mixerProfileModePresent) { + return false; + } + + switch (requiredAction) { + case MIXERAT_REQUEST_RTH: + return automatedSwitch && stateMultirotor && targetProfileIsAirplane; + + case MIXERAT_REQUEST_LAND: + return automatedSwitch && stateAirplane && targetProfileIsMultirotor; + +#ifdef USE_AUTO_TRANSITION + case MIXERAT_REQUEST_MISSION_TO_FW: + case MIXERAT_REQUEST_MANUAL_TO_FW: + return stateMultirotor && targetProfileIsAirplane; + + case MIXERAT_REQUEST_MISSION_TO_MC: + case MIXERAT_REQUEST_MANUAL_TO_MC: + return stateAirplane && targetProfileIsMultirotor; + + case MIXERAT_REQUEST_FW_TO_MC_PROTECTION: + return automatedSwitch && stateAirplane && targetProfileIsMultirotor; +#endif + + default: + return false; + } +} + +static inline bool mixerTransitionRequestAllowedDuringFailsafe(mixerProfileATRequest_e requiredAction) +{ + switch (requiredAction) { + case MIXERAT_REQUEST_RTH: + case MIXERAT_REQUEST_LAND: + return true; + +#ifdef USE_AUTO_TRANSITION + case MIXERAT_REQUEST_FW_TO_MC_PROTECTION: + return true; +#endif + + default: + return false; + } +} + +#ifdef USE_AUTO_TRANSITION +static inline bool mixerTransitionFwToMcProtectionTriggered( + const bool armed, + const bool stateAirplane, + const uint16_t thresholdCmS, + const bool trustedAirspeedAvailable, + const float airspeedCmS) +{ + return armed && + stateAirplane && + thresholdCmS > 0 && + trustedAirspeedAvailable && + airspeedCmS <= thresholdCmS; +} + +#endif + +static inline bool mixerTransitionShouldAbortForFailsafe( + const mixerProfileATRequest_e requiredAction, + const bool postSwitchActive, + const bool hotSwitchDone) +{ + if (mixerTransitionRequestAllowedDuringFailsafe(requiredAction)) { + return false; + } + + // Once the target profile is already active, finishing the remaining output + // ramp is safer than cancelling into an abrupt scale reset. + return !(postSwitchActive && hotSwitchDone); +} + +#ifdef USE_AUTO_TRANSITION +static inline float mixerTransitionComputeMotorRampProgress( + bool dynamicMixerEnabled, + uint16_t scaleRampTimeMs, + uint32_t elapsedMs) +{ + if (!dynamicMixerEnabled || scaleRampTimeMs == 0) { + return 1.0f; + } + + return mixerTransitionClamp((float)elapsedMs / (float)scaleRampTimeMs, 0.0f, 1.0f); +} + +static inline uint16_t mixerTransitionComputeServoHandoffDurationMs( + bool dynamicMixerEnabled, + uint16_t scaleRampTimeMs, + uint32_t elapsedMs) +{ + (void)dynamicMixerEnabled; + (void)elapsedMs; + + if (scaleRampTimeMs == 0) { + return 0; + } + + return scaleRampTimeMs; +} + +static inline bool mixerTransitionServoHandoffExpired(uint32_t elapsedMs, uint16_t durationMs) +{ + return durationMs == 0 || elapsedMs >= durationMs; +} + +static inline float mixerTransitionServoHandoffProgress(uint32_t elapsedMs, uint16_t durationMs) +{ + if (durationMs == 0) { + return 1.0f; + } + + return mixerTransitionClamp((float)elapsedMs / (float)durationMs, 0.0f, 1.0f); +} + +static inline int16_t mixerTransitionRoundFloatToInt16(float value) +{ + return (int16_t)(value >= 0.0f ? value + 0.5f : value - 0.5f); +} + +static inline void mixerTransitionClearAirspeedProgressConfirmation(mixerTransitionAirspeedProgressFilter_t *filter) +{ + if (!filter) { + return; + } + + filter->active = false; + filter->windowMature = false; + filter->bucketCount = 0; + filter->nextBucket = 0; + filter->windowStartTime = 0; + filter->recentMinProgress = 0.0f; + filter->recentMaxProgress = 0.0f; +} + +static inline void mixerTransitionResetAirspeedProgressFilter(mixerTransitionAirspeedProgressFilter_t *filter) +{ + if (!filter) { + return; + } + + mixerTransitionClearAirspeedProgressConfirmation(filter); + filter->acceptedProgressUpdateTime = 0; + filter->acceptedProgressUpdateValid = false; +} + +static inline float mixerTransitionSlewAcceptedProgress( + float acceptedProgress, + float targetProgress, + timeMs_t currentTimeMs, + uint16_t durationMs, + bool holdAtFirstUpdate, + mixerTransitionAirspeedProgressFilter_t *filter) +{ + if (!filter || durationMs == 0) { + return targetProgress; + } + + if (!filter->acceptedProgressUpdateValid) { + filter->acceptedProgressUpdateTime = currentTimeMs; + filter->acceptedProgressUpdateValid = true; + return holdAtFirstUpdate ? acceptedProgress : targetProgress; + } + + const uint32_t elapsedMs = currentTimeMs - filter->acceptedProgressUpdateTime; + filter->acceptedProgressUpdateTime = currentTimeMs; + const float maximumStep = (float)elapsedMs / (float)durationMs; + + if (targetProgress > acceptedProgress) { + const float nextProgress = acceptedProgress + maximumStep; + return nextProgress < targetProgress ? nextProgress : targetProgress; + } + + const float nextProgress = acceptedProgress - maximumStep; + return nextProgress > targetProgress ? nextProgress : targetProgress; +} + +static inline bool mixerTransitionAirspeedProgressEndpointConfirmed( + const mixerTransitionAirspeedProgressFilter_t *filter) +{ + return filter && + filter->active && + filter->windowMature && + filter->recentMinProgress >= 1.0f; +} + +static inline bool mixerTransitionAirspeedProgressHasRecentSample( + const mixerTransitionAirspeedProgressFilter_t *filter, + timeMs_t currentTimeMs, + uint16_t maximumAgeMs) +{ + if (!filter || !filter->active || filter->bucketCount == 0 || maximumAgeMs == 0) { + return false; + } + + const uint8_t currentBucket = filter->nextBucket == 0 ? + MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT - 1 : + filter->nextBucket - 1; + + return (currentTimeMs - filter->bucketEndTime[currentBucket]) < maximumAgeMs; +} + +static inline float mixerTransitionResolveHandoffProgress( + bool dynamicMixerEnabled, + bool usedAirspeed, + float previousHandoffProgress, + float rawProgress, + timeMs_t currentTimeMs, + uint16_t confirmationTimeMs, + uint16_t bucketTimeMs, + mixerTransitionAirspeedProgressFilter_t *filter) +{ + const float clampedProgress = mixerTransitionClamp(rawProgress, 0.0f, 1.0f); + + if (!usedAirspeed || !filter || confirmationTimeMs == 0 || bucketTimeMs == 0) { + mixerTransitionClearAirspeedProgressConfirmation(filter); + if (!dynamicMixerEnabled) { + return 1.0f; + } + const float timerFallbackProgress = previousHandoffProgress > clampedProgress ? + previousHandoffProgress : clampedProgress; + return mixerTransitionSlewAcceptedProgress( + previousHandoffProgress, + timerFallbackProgress, + currentTimeMs, + confirmationTimeMs, + false, + filter); + } + + if (!filter->active) { + filter->active = true; + filter->bucketCount = 0; + filter->nextBucket = 0; + filter->windowStartTime = currentTimeMs; + // A returning airspeed source starts a new confirmation period. Do + // not use a timestamp from timer fallback to apply its result at once. + filter->acceptedProgressUpdateValid = false; + } + + uint8_t currentBucket = filter->nextBucket == 0 ? + MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT - 1 : + filter->nextBucket - 1; + + if (filter->bucketCount > 0 && + (currentTimeMs - filter->bucketEndTime[currentBucket]) > (uint32_t)bucketTimeMs * 2U) { + mixerTransitionResetAirspeedProgressFilter(filter); + filter->active = true; + filter->windowStartTime = currentTimeMs; + } + + currentBucket = filter->nextBucket == 0 ? + MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT - 1 : + filter->nextBucket - 1; + const bool startNewBucket = filter->bucketCount == 0 || + (currentTimeMs - filter->bucketStartTime[currentBucket]) >= bucketTimeMs; + + if (startNewBucket) { + currentBucket = filter->nextBucket; + filter->bucketStartTime[currentBucket] = currentTimeMs; + filter->bucketEndTime[currentBucket] = currentTimeMs; + filter->bucketMinProgress[currentBucket] = clampedProgress; + filter->bucketMaxProgress[currentBucket] = clampedProgress; + filter->nextBucket = (filter->nextBucket + 1) % MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT; + if (filter->bucketCount < MIXER_TRANSITION_AIRSPEED_PROGRESS_BUCKET_COUNT) { + filter->bucketCount++; + } + } else { + filter->bucketEndTime[currentBucket] = currentTimeMs; + if (clampedProgress < filter->bucketMinProgress[currentBucket]) { + filter->bucketMinProgress[currentBucket] = clampedProgress; + } + if (clampedProgress > filter->bucketMaxProgress[currentBucket]) { + filter->bucketMaxProgress[currentBucket] = clampedProgress; + } + } + + const float acceptedProgress = mixerTransitionClamp(previousHandoffProgress, 0.0f, 1.0f); + filter->windowMature = false; + if ((currentTimeMs - filter->windowStartTime) < confirmationTimeMs) { + return dynamicMixerEnabled ? acceptedProgress : 1.0f; + } + + bool hasRecentBucket = false; + float recentMinProgress = 1.0f; + float recentMaxProgress = 0.0f; + + for (uint8_t i = 0; i < filter->bucketCount; i++) { + if ((currentTimeMs - filter->bucketEndTime[i]) > confirmationTimeMs) { + continue; + } + + hasRecentBucket = true; + if (filter->bucketMinProgress[i] < recentMinProgress) { + recentMinProgress = filter->bucketMinProgress[i]; + } + if (filter->bucketMaxProgress[i] > recentMaxProgress) { + recentMaxProgress = filter->bucketMaxProgress[i]; + } + } + + if (!hasRecentBucket) { + return dynamicMixerEnabled ? acceptedProgress : 1.0f; + } + + filter->windowMature = true; + filter->recentMinProgress = recentMinProgress; + filter->recentMaxProgress = recentMaxProgress; + + if (!dynamicMixerEnabled) { + return 1.0f; + } + + // Advance only when the complete confirmation window stayed above the + // accepted progress, and retreat only when it stayed below it. The + // confirmation window can establish a large delta at once, so rate-limit + // the accepted value afterwards to avoid a common-mode lift step. + float targetProgress = acceptedProgress; + if (recentMinProgress > acceptedProgress) { + targetProgress = recentMinProgress; + } else if (recentMaxProgress < acceptedProgress) { + targetProgress = recentMaxProgress; + } + + return mixerTransitionSlewAcceptedProgress( + acceptedProgress, + targetProgress, + currentTimeMs, + confirmationTimeMs, + true, + filter); +} + +static inline float mixerTransitionBlendScale(float from, float to, float progress) +{ + return from + (to - from) * mixerTransitionClamp(progress, 0.0f, 1.0f); +} + +static inline int16_t mixerTransitionBlendCapturedMotorOutput( + int16_t capturedOutput, + int16_t targetOutput, + float progress) +{ + const float holdScale = 1.0f - mixerTransitionClamp(progress, 0.0f, 1.0f); + const int32_t blendedOutput = mixerTransitionRoundFloatToInt16( + targetOutput + (capturedOutput - targetOutput) * holdScale); + const int16_t low = targetOutput < capturedOutput ? targetOutput : capturedOutput; + const int16_t high = targetOutput > capturedOutput ? targetOutput : capturedOutput; + + if (blendedOutput < low) { + return low; + } + + if (blendedOutput > high) { + return high; + } + + return (int16_t)blendedOutput; +} + +static inline uint16_t mixerTransitionComputePostSwitchFadeDurationMs( + uint16_t scaleRampTimeMs, + uint16_t motorMask, + uint16_t fastToCurrentMotorMask) +{ + if (scaleRampTimeMs == 0 || motorMask == 0) { + return 0; + } + + // A new MC lift output is an output-continuity guard, not a deliberate + // slow authority fade. Do not leave the MC PID handoff sluggish merely + // because the user selected a long pusher/lift transition ramp. + if ((motorMask & ~fastToCurrentMotorMask) == 0) { + return scaleRampTimeMs < MIXER_TRANSITION_MC_LIFT_OUTPUT_HANDOFF_MAX_MS ? + scaleRampTimeMs : MIXER_TRANSITION_MC_LIFT_OUTPUT_HANDOFF_MAX_MS; + } + + return scaleRampTimeMs; +} + +static inline mixerTransitionScaleState_t mixerTransitionComputeScales( + bool dynamicMixerEnabled, + mixerProfileATDirection_e direction, + float liftFloor, + float mcFloor, + float fwFloor, + float handoffProgress, + float motorRampProgress) +{ + mixerTransitionScaleState_t scales = { + .pusherScale = 1.0f, + .liftScale = 1.0f, + .mcAuthorityScale = 1.0f, + .fwAuthorityScale = 1.0f, + .blendToFw = 1.0f, + }; + + if (!dynamicMixerEnabled) { + return scales; + } + + liftFloor = mixerTransitionClamp(liftFloor, 0.0f, 1.0f); + mcFloor = mixerTransitionClamp(mcFloor, 0.0f, 1.0f); + fwFloor = mixerTransitionClamp(fwFloor, 0.0f, 1.0f); + + if (direction == MIXERAT_DIRECTION_TO_FW) { + scales.pusherScale = mixerTransitionBlendScale(0.0f, 1.0f, motorRampProgress); + scales.liftScale = mixerTransitionBlendScale(1.0f, liftFloor, handoffProgress); + scales.mcAuthorityScale = mixerTransitionBlendScale(1.0f, mcFloor, handoffProgress); + scales.fwAuthorityScale = mixerTransitionBlendScale(fwFloor, 1.0f, handoffProgress); + } else if (direction == MIXERAT_DIRECTION_TO_MC) { + scales.pusherScale = mixerTransitionBlendScale(1.0f, 0.0f, motorRampProgress); + scales.liftScale = mixerTransitionBlendScale(liftFloor, 1.0f, motorRampProgress); + scales.mcAuthorityScale = mixerTransitionBlendScale(mcFloor, 1.0f, motorRampProgress); + scales.fwAuthorityScale = mixerTransitionBlendScale(1.0f, fwFloor, handoffProgress); + } + + scales.blendToFw = mixerTransitionClamp(scales.fwAuthorityScale, 0.0f, 1.0f); + return scales; +} + +static inline mixerTransitionHotSwitchProgress_t mixerTransitionEvaluateHotSwitch( + mixerProfileATDirection_e direction, + uint16_t airspeedThresholdCmS, + bool trustedAirspeedAvailable, + float currentAirspeedCmS, + bool transitionStartAirspeedCaptured, + float transitionStartAirspeedCmS, + uint32_t elapsedMs, + uint32_t transitionTimerMs) +{ + mixerTransitionHotSwitchProgress_t result = { + .readyForHotSwitch = false, + .usedAirspeed = false, + .transitionStartAirspeedCaptured = transitionStartAirspeedCaptured, + .transitionStartAirspeedCmS = transitionStartAirspeedCmS, + .progress = 0.0f, + }; + + if (direction == MIXERAT_DIRECTION_NONE) { + return result; + } + + if (airspeedThresholdCmS > 0 && trustedAirspeedAvailable) { + result.usedAirspeed = true; + + if (direction == MIXERAT_DIRECTION_TO_FW) { + result.progress = mixerTransitionClamp(currentAirspeedCmS / airspeedThresholdCmS, 0.0f, 1.0f); + result.readyForHotSwitch = currentAirspeedCmS >= airspeedThresholdCmS; + return result; + } + + if (!result.transitionStartAirspeedCaptured) { + result.transitionStartAirspeedCmS = currentAirspeedCmS; + result.transitionStartAirspeedCaptured = true; + } + + if (result.transitionStartAirspeedCmS <= airspeedThresholdCmS) { + // The transition started inside the MC speed envelope, so there is + // no deceleration range to normalize against. Keep progress at the + // endpoint only while the current sample also satisfies the limit; + // otherwise the shared confirmation window would forget a later + // speed increase and could allow an early hot-switch. + result.progress = currentAirspeedCmS <= airspeedThresholdCmS ? + 1.0f : + mixerTransitionClamp((float)airspeedThresholdCmS / currentAirspeedCmS, 0.0f, 1.0f); + } else { + result.progress = mixerTransitionClamp( + (result.transitionStartAirspeedCmS - currentAirspeedCmS) / + (result.transitionStartAirspeedCmS - airspeedThresholdCmS), + 0.0f, + 1.0f); + } + + result.readyForHotSwitch = currentAirspeedCmS <= airspeedThresholdCmS; + return result; + } + + result.progress = transitionTimerMs > 0 ? + mixerTransitionClamp((float)elapsedMs / (float)transitionTimerMs, 0.0f, 1.0f) : + 1.0f; + result.readyForHotSwitch = elapsedMs >= transitionTimerMs; + return result; +} + +static inline mixerTransitionPostSwitchFadeMask_t mixerTransitionComputePostSwitchFadeMask( + bool dynamicMixerEnabled, + uint16_t scaleRampTimeMs, + mixerProfileATDirection_e direction, + bool currentProfileIsMultirotor, + uint8_t motorCount, + const motorMixer_t *currentMotorMixer, + const motorMixer_t *targetMotorMixer) +{ + mixerTransitionPostSwitchFadeMask_t mask = { 0, 0, 0 }; + + if (!dynamicMixerEnabled || + scaleRampTimeMs == 0 || + direction == MIXERAT_DIRECTION_NONE || + !currentMotorMixer || + !targetMotorMixer) { + return mask; + } + + for (uint8_t i = 0; i < motorCount && i < MAX_SUPPORTED_MOTORS; i++) { + const bool currentMotorActive = currentMotorMixer[i].throttle > 0.0f; + const bool targetMotorActive = targetMotorMixer[i].throttle > 0.0f; + const bool oldLiftMotor = direction == MIXERAT_DIRECTION_TO_FW && + currentProfileIsMultirotor && + currentMotorActive && + !targetMotorActive; + const bool oldPusherMotor = direction == MIXERAT_DIRECTION_TO_MC && + !currentProfileIsMultirotor && + currentMotorActive && + !targetMotorActive; + const bool targetFwPusherMotor = direction == MIXERAT_DIRECTION_TO_FW && + currentProfileIsMultirotor && + !currentMotorActive && + targetMotorActive; + const bool targetMcLiftMotor = direction == MIXERAT_DIRECTION_TO_MC && + !currentProfileIsMultirotor && + !currentMotorActive && + targetMotorActive; + + if (oldLiftMotor || oldPusherMotor || targetFwPusherMotor || targetMcLiftMotor) { + mask.motorMask |= (1U << i); + if (targetFwPusherMotor || targetMcLiftMotor) { + mask.toCurrentMotorMask |= (1U << i); + } + if (targetMcLiftMotor) { + mask.fastToCurrentMotorMask |= (1U << i); + } + } + } + + return mask; +} +#endif + +#ifdef USE_AUTO_TRANSITION +static inline float mixerTransitionComputeServoBlendToFw( + bool legacyManualTransitionActive, + bool transitionMixingActive, + bool autoTransitionActive, + bool postSwitchFadeToFwActive, + bool dynamicMixerEnabled, + mixerProfileATDirection_e direction, + uint16_t scaleRampTimeMs, + uint32_t elapsedMs) +{ + if (legacyManualTransitionActive) { + return transitionMixingActive ? 1.0f : 0.0f; + } + + if (postSwitchFadeToFwActive) { + return 1.0f; + } + + if (!autoTransitionActive || direction == MIXERAT_DIRECTION_NONE) { + return 0.0f; + } + + if (!dynamicMixerEnabled) { + return transitionMixingActive ? 1.0f : 0.0f; + } + + const float progress = mixerTransitionComputeMotorRampProgress( + dynamicMixerEnabled, + scaleRampTimeMs, + elapsedMs); + + if (direction == MIXERAT_DIRECTION_TO_MC) { + return 1.0f - progress; + } + + return progress; +} +#endif + +static inline int16_t mixerTransitionBlendToServoInput(float blendToFw) +{ + if (blendToFw <= 0.0f) { + return 0; + } + + if (blendToFw >= 1.0f) { + return 500; + } + + return (int16_t)(blendToFw * 500.0f + 0.5f); +} + +static inline int16_t mixerTransitionUpdateServoInput( + int16_t previousInput, + bool legacyManualTransitionActive, + bool transitionMixingActive, + bool autoTransitionActive, + bool postSwitchFadeToFwActive, + bool transitionToFw, + float blendToFw) +{ + int16_t desiredInput = 0; + + if (legacyManualTransitionActive) { + desiredInput = transitionMixingActive ? 500 : 0; + } else if (postSwitchFadeToFwActive) { + desiredInput = 500; + } else if (transitionMixingActive) { + desiredInput = autoTransitionActive ? mixerTransitionBlendToServoInput(blendToFw) : 500; + } else if (autoTransitionActive && transitionToFw) { + desiredInput = mixerTransitionBlendToServoInput(blendToFw); + } + + // Only the new auto-transition MC->FW path needs the anti-reverse latch. + // Legacy mode must keep its original fixed 0/500 endpoint behaviour. + if (transitionToFw && + desiredInput < previousInput && + !legacyManualTransitionActive && + (transitionMixingActive || autoTransitionActive || postSwitchFadeToFwActive)) { + return previousInput; + } + + return desiredInput; +} + +static inline int16_t mixerTransitionBlendCapturedServoOutput( + int16_t capturedOutput, + int16_t currentOutput, + float progress) +{ + const float holdScale = 1.0f - mixerTransitionClamp(progress, 0.0f, 1.0f); + const float fadedOutput = currentOutput + (capturedOutput - currentOutput) * holdScale; + const int16_t minOutput = capturedOutput < currentOutput ? capturedOutput : currentOutput; + const int16_t maxOutput = capturedOutput > currentOutput ? capturedOutput : currentOutput; + + if (fadedOutput <= minOutput) { + return minOutput; + } + + if (fadedOutput >= maxOutput) { + return maxOutput; + } + + return (int16_t)(fadedOutput + (fadedOutput >= 0.0f ? 0.5f : -0.5f)); +} diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index bfdc890c602..e89c53ca532 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -122,6 +122,38 @@ typedef struct { uint16_t pidSumLimit; } pidState_t; +#ifdef USE_AUTO_TRANSITION +typedef struct { + float kP; + float kI; + float kD; + float kFF; + float kCD; + float kT; + float gyroRate; + float rateTarget; + float errorGyroIf; + float errorGyroIfLimit; + pt1Filter_t angleFilterState; + rateLimitFilter_t axisAccelFilter; + pt1Filter_t ptermLpfState; + filter_t dtermLpfState; + float previousRateTarget; + float previousRateGyro; + pt3Filter_t rateTargetFilter; +#ifdef USE_D_BOOST + pt1Filter_t dBoostLpf; + biquadFilter_t dBoostGyroLpf; + float dBoostTargetAcceleration; +#endif + filterApply3FnPtr ptermFilterApplyFn; +#ifdef USE_SMITH_PREDICTOR + smithPredictor_t smithPredictor; +#endif + fwPidAttenuation_t attenuation; +} autoTransitionTargetPidState_t; +#endif + STATIC_FASTRAM bool pidFiltersConfigured = false; static EXTENDED_FASTRAM float headingHoldCosZLimit; static EXTENDED_FASTRAM int16_t headingHoldTarget; @@ -131,6 +163,9 @@ static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter; // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied STATIC_FASTRAM bool pidGainsUpdateRequired= true; FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; +#ifdef USE_AUTO_TRANSITION +FASTRAM int16_t autoTransitionTargetAxisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; +#endif #ifdef USE_BLACKBOX int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT]; @@ -141,8 +176,19 @@ int32_t axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT]; #endif static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT]; +#ifdef USE_AUTO_TRANSITION +static EXTENDED_FASTRAM autoTransitionTargetPidState_t autoTransitionTargetPidState[FLIGHT_DYNAMICS_INDEX_COUNT]; +#endif static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; static EXTENDED_FASTRAM uint8_t itermRelax; +#ifdef USE_AUTO_TRANSITION +static EXTENDED_FASTRAM pt1Filter_t autoTransitionTargetHeadingHoldRateFilter; +static EXTENDED_FASTRAM pt1Filter_t autoTransitionTargetTpaFilter; +static EXTENDED_FASTRAM filterApplyFnPtr autoTransitionTargetDTermLpfFilterApplyFn; +static EXTENDED_FASTRAM uint8_t autoTransitionTargetYawLpfHz; +static EXTENDED_FASTRAM int8_t autoTransitionTargetControlProfileIndex = -1; +static EXTENDED_FASTRAM bool autoTransitionTargetFixedWing = false; +#endif #ifdef USE_ANTIGRAVITY static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf; @@ -184,7 +230,12 @@ static EXTENDED_FASTRAM timeMs_t pidLoopNowMs; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11); +static void applyItermLimiting(pidState_t *pidState); +static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination); +float pidRcCommandToRate(int16_t stick, uint8_t rate); +int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis); + +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 12); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -428,6 +479,11 @@ void pidResetErrorAccumulators(void) for (int axis = 0; axis < 3; axis++) { pidState[axis].errorGyroIf = 0.0f; pidState[axis].errorGyroIfLimit = 0.0f; +#ifdef USE_AUTO_TRANSITION + autoTransitionTargetPidState[axis].errorGyroIf = 0.0f; + autoTransitionTargetPidState[axis].errorGyroIfLimit = 0.0f; + autoTransitionTargetAxisPID[axis] = 0; +#endif } } @@ -447,6 +503,744 @@ float getAxisIterm(uint8_t axis) return pidState[axis].errorGyroIf; } +#ifdef USE_AUTO_TRANSITION +static void NOINLINE resetAutoTransitionTargetPidState(void) +{ + memset(autoTransitionTargetPidState, 0, sizeof(autoTransitionTargetPidState)); + memset(autoTransitionTargetAxisPID, 0, sizeof(autoTransitionTargetAxisPID)); + memset(&autoTransitionTargetHeadingHoldRateFilter, 0, sizeof(autoTransitionTargetHeadingHoldRateFilter)); + memset(&autoTransitionTargetTpaFilter, 0, sizeof(autoTransitionTargetTpaFilter)); + autoTransitionTargetControlProfileIndex = -1; + autoTransitionTargetFixedWing = false; + autoTransitionTargetDTermLpfFilterApplyFn = (filterApplyFnPtr)nullFilterApply; + autoTransitionTargetYawLpfHz = 0; +} + +static bool isAutoTransitionTargetPidActive(void) +{ + if (!(isMixerTransitionMixing && + mixerATIsActive() && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT)) { + return false; + } + + const flyingPlatformType_e targetPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; + + return (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && + isMultirotorTypePlatform(currentMixerConfig.platformType) && + targetPlatformType == PLATFORM_AIRPLANE) || + (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && + currentMixerConfig.platformType == PLATFORM_AIRPLANE && + isMultirotorTypePlatform(targetPlatformType)); +} + +static bool isAutoTransitionTargetFixedWing(void) +{ + return nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT && + mixerConfigByIndex(nextMixerProfileIndex)->platformType == PLATFORM_AIRPLANE; +} + +static uint8_t getAutoTransitionTargetControlProfileIndex(void) +{ + if (currentMixerConfig.controlProfileLinking && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_CONTROL_PROFILE_COUNT && + nextMixerProfileIndex < MAX_PROFILE_COUNT) { + return nextMixerProfileIndex; + } + + return getConfigProfile(); +} + +static const controlConfig_t *getAutoTransitionTargetControlProfile(uint8_t profileIndex) +{ + if (currentMixerConfig.controlProfileLinking && + profileIndex < MAX_CONTROL_PROFILE_COUNT) { + return controlProfiles(profileIndex); + } + + return currentControlProfile; +} + +static const pidProfile_t *getAutoTransitionTargetPidProfile(uint8_t profileIndex) +{ + if (currentMixerConfig.controlProfileLinking && profileIndex < MAX_PROFILE_COUNT) { + const pgRegistry_t *pidProfileRegistry = pgFind(PG_PID_PROFILE); + if (pidProfileRegistry) { + return (const pidProfile_t *)(pidProfileRegistry->address + (pgSize(pidProfileRegistry) * profileIndex)); + } + } + + return pidProfile(); +} + +static void initAutoTransitionTargetPidState(const uint8_t profileIndex, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, const bool targetIsFixedWing) +{ + const uint32_t refreshRate = getLooptime(); + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; + + resetAutoTransitionTargetPidState(); + + autoTransitionTargetControlProfileIndex = profileIndex; + autoTransitionTargetFixedWing = targetIsFixedWing; + autoTransitionTargetYawLpfHz = targetPidProfile->yaw_lpf_hz; + + assignFilterApplyFn(targetPidProfile->dterm_lpf_type, targetPidProfile->dterm_lpf_hz, &autoTransitionTargetDTermLpfFilterApplyFn); + + if (controlProfile->throttle.fixedWingTauMs > 0) { + pt1FilterInit(&autoTransitionTargetTpaFilter, 1.0f, HZ2S(TASK_AUX_RATE_HZ)); + pt1FilterSetTimeConstant(&autoTransitionTargetTpaFilter, MS2S(controlProfile->throttle.fixedWingTauMs)); + pt1FilterReset(&autoTransitionTargetTpaFilter, getThrottleIdleValue()); + } + + pt1FilterSetCutoff(&autoTransitionTargetHeadingHoldRateFilter, HEADING_HOLD_ERROR_LPF_FREQ); + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { +#ifdef USE_D_BOOST + autoTransitionTargetPidState[axis].dBoostTargetAcceleration = controlProfile->stabilized.rates[axis] * 10 * 10; + pt1FilterSetCutoff(&autoTransitionTargetPidState[axis].dBoostLpf, D_BOOST_LPF_HZ); +#endif + + pt1FilterSetCutoff(&autoTransitionTargetPidState[axis].angleFilterState, targetPidBank->pid[PID_LEVEL].I); + pt3FilterInit(&autoTransitionTargetPidState[axis].rateTargetFilter, pt3FilterGain(targetPidProfile->controlDerivativeLpfHz, US2S(refreshRate))); + + if (axis == FD_YAW && autoTransitionTargetYawLpfHz) { + pt1FilterSetCutoff(&autoTransitionTargetPidState[axis].ptermLpfState, autoTransitionTargetYawLpfHz); + autoTransitionTargetPidState[axis].ptermFilterApplyFn = (filterApply3FnPtr)pt1FilterApply3; + } else { + autoTransitionTargetPidState[axis].ptermFilterApplyFn = (filterApply3FnPtr)nullFilterApply3; + } + + initFilter(targetPidProfile->dterm_lpf_type, &autoTransitionTargetPidState[axis].dtermLpfState, targetPidProfile->dterm_lpf_hz, refreshRate); + +#ifdef USE_D_BOOST + biquadFilterInitLPF(&autoTransitionTargetPidState[axis].dBoostGyroLpf, targetPidProfile->dBoostGyroDeltaLpfHz, refreshRate); +#endif + +#ifdef USE_SMITH_PREDICTOR + smithPredictorInit( + &autoTransitionTargetPidState[axis].smithPredictor, + targetPidProfile->smithPredictorDelay, + targetPidProfile->smithPredictorStrength, + targetPidProfile->smithPredictorFilterHz, + refreshRate + ); +#endif + } +} + +static float calculateAutoTransitionTargetAirspeedTPAFactor(const pidProfile_t *targetPidProfile, const controlConfig_t *controlProfile) +{ + const float airspeed = constrainf(getAirspeedEstimate(), 100.0f, 20000.0f); + const float referenceAirspeed = targetPidProfile->fixedWingReferenceAirspeed; + float tpaFactor = powf(referenceAirspeed / airspeed, controlProfile->throttle.apa_pow / 100.0f); + + return constrainf(tpaFactor, 0.3f, 2.0f); +} + +static float calculateAutoTransitionTargetAirspeedITermFactor(const pidProfile_t *targetPidProfile, const controlConfig_t *controlProfile) +{ + const float airspeed = constrainf(getAirspeedEstimate(), 100.0f, 20000.0f); + const float referenceAirspeed = targetPidProfile->fixedWingReferenceAirspeed; + const float apaPow = controlProfile->throttle.apa_pow; + + if (apaPow <= 100.0f) { + return 1.0f; + } + + return constrainf(powf(referenceAirspeed / airspeed, (apaPow / 100.0f) - 1.0f), 0.3f, 1.5f); +} + +static uint16_t calculateAutoTransitionTargetTPAThrottle(const controlConfig_t *controlProfile) +{ + if (controlProfile->throttle.fixedWingTauMs > 0) { + static const fpVector3_t vDown = { .v = { 0.0f, 0.0f, 1.0f } }; + fpVector3_t vForward = { .v = { HeadVecEFFiltered.x, -HeadVecEFFiltered.y, -HeadVecEFFiltered.z } }; + const float groundCos = vectorDotProduct(&vForward, &vDown); + const int16_t throttleAdjustment = controlProfile->throttle.tpa_pitch_compensation * groundCos * 90.0f / 1.57079632679f; + const uint16_t throttleAdjusted = rcCommand[THROTTLE] + constrain(throttleAdjustment, -1000, 1000); + return pt1FilterApply(&autoTransitionTargetTpaFilter, constrain(throttleAdjusted, 1000, 2000)); + } + + return rcCommand[THROTTLE]; +} + +static float calculateAutoTransitionTargetTPAFactor(const controlConfig_t *controlProfile) +{ + const uint16_t throttle = calculateAutoTransitionTargetTPAThrottle(controlProfile); + float tpaFactor; + + if (controlProfile->throttle.dynPID != 0 && + controlProfile->throttle.pa_breakpoint > getThrottleIdleValue() && + !FLIGHT_MODE(AUTO_TUNE) && + ARMING_FLAG(ARMED)) { + if (throttle > getThrottleIdleValue()) { + tpaFactor = 0.5f + ((float)(controlProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); + } else { + tpaFactor = 2.0f; + } + + tpaFactor = 1.0f + (tpaFactor - 1.0f) * (controlProfile->throttle.dynPID / 100.0f); + tpaFactor = constrainf(tpaFactor, 0.3f, 2.0f); + } else { + tpaFactor = 1.0f; + } + + return tpaFactor; +} + +static float calculateAutoTransitionTargetMultirotorTPAFactor(const controlConfig_t *controlProfile) +{ + const uint16_t throttle = rcCommand[THROTTLE]; + + if (controlProfile->throttle.dynPID == 0 || throttle < controlProfile->throttle.pa_breakpoint) { + return 1.0f; + } + + if (throttle < getMaxThrottle()) { + return (100 - (uint16_t)controlProfile->throttle.dynPID * (throttle - controlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - controlProfile->throttle.pa_breakpoint)) / 100.0f; + } + + return (100 - constrain(controlProfile->throttle.dynPID, 0, 100)) / 100.0f; +} + +static void updateAutoTransitionTargetPIDCoefficients(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, const bool targetIsFixedWing) +{ + float tpaFactor = 1.0f; + float iTermFactor = 1.0f; + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; + + if (targetIsFixedWing) { + if (controlProfile->throttle.apa_pow > 0 && + pitotGetValidForAirspeed()) { + tpaFactor = calculateAutoTransitionTargetAirspeedTPAFactor(targetPidProfile, controlProfile); + iTermFactor = calculateAutoTransitionTargetAirspeedITermFactor(targetPidProfile, controlProfile); + } else { + tpaFactor = calculateAutoTransitionTargetTPAFactor(controlProfile); + iTermFactor = tpaFactor; + } + } else { + tpaFactor = calculateAutoTransitionTargetMultirotorTPAFactor(controlProfile); + iTermFactor = 1.0f; + } + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + const float axisTPA = (!targetIsFixedWing && axis == FD_YAW && !controlProfile->throttle.dynPID_on_YAW) ? 1.0f : tpaFactor; + + autoTransitionTargetPidState[axis].kP = targetPidBank->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA; + autoTransitionTargetPidState[axis].kI = targetPidBank->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * iTermFactor; + autoTransitionTargetPidState[axis].kD = targetPidBank->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA; + + if (targetIsFixedWing) { + autoTransitionTargetPidState[axis].kFF = targetPidBank->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; + autoTransitionTargetPidState[axis].kCD = 0.0f; + autoTransitionTargetPidState[axis].kT = 0.0f; + } else { + autoTransitionTargetPidState[axis].kFF = 0.0f; + autoTransitionTargetPidState[axis].kCD = (targetPidBank->pid[axis].FF / FP_PID_RATE_D_FF_MULTIPLIER * axisTPA) / (getLooptime() * 0.000001f); + if (targetPidBank->pid[axis].P != 0 && targetPidBank->pid[axis].I != 0) { + autoTransitionTargetPidState[axis].kT = 2.0f / ((autoTransitionTargetPidState[axis].kP / autoTransitionTargetPidState[axis].kI) + (autoTransitionTargetPidState[axis].kD / autoTransitionTargetPidState[axis].kP)); + } else { + autoTransitionTargetPidState[axis].kT = 0.0f; + } + } + } +} + +static float calcAutoTransitionTargetHorizonRateMagnitude(const pidProfile_t *targetPidProfile, const bool targetIsFixedWing) +{ + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH)); + const float mostDeflectedStickPos = constrain(MAX(stickPosAil, stickPosEle), 0, 500) / 500.0f; + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; + const float modeTransitionStickPos = constrain(targetPidBank->pid[PID_LEVEL].D, 0, 100) / 100.0f; + + if (modeTransitionStickPos <= 0.0f) { + return 1.0f; + } + + if (mostDeflectedStickPos <= modeTransitionStickPos) { + return mostDeflectedStickPos / modeTransitionStickPos; + } + + return 1.0f; +} + +static uint8_t getAutoTransitionTargetHeadingHoldState(const pidProfile_t *targetPidProfile) +{ + const float headingHoldCosLimit = + cos_approx(DECIDEGREES_TO_RADIANS(targetPidProfile->max_angle_inclination[FD_ROLL])) * + cos_approx(DECIDEGREES_TO_RADIANS(targetPidProfile->max_angle_inclination[FD_PITCH])); + + if (calculateCosTiltAngle() < headingHoldCosLimit) { + return HEADING_HOLD_DISABLED; + } + + const int navHeadingState = navigationGetHeadingControlState(); + if (navHeadingState != NAV_HEADING_CONTROL_NONE) { + if (navHeadingState == NAV_HEADING_CONTROL_AUTO) { + return HEADING_HOLD_ENABLED; + } + } else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) { + return HEADING_HOLD_ENABLED; + } + + return HEADING_HOLD_UPDATE_HEADING; +} + +static float pidAutoTransitionTargetHeadingHold(const pidProfile_t *targetPidProfile, const bool targetIsFixedWing, float dT) +{ + int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget; + + if (error > 180) { + error -= 360; + } else if (error < -180) { + error += 360; + } + + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; + float headingHoldRate = error * targetPidBank->pid[PID_HEADING].P / 30.0f; + headingHoldRate = constrainf(headingHoldRate, -targetPidProfile->heading_hold_rate_limit, targetPidProfile->heading_hold_rate_limit); + headingHoldRate = pt1FilterApply3(&autoTransitionTargetHeadingHoldRateFilter, headingHoldRate, dT); + + return headingHoldRate; +} + +static float computeAutoTransitionTargetPidLevelTarget(const pidProfile_t *targetPidProfile, const bool targetIsFixedWing, flight_dynamics_index_t axis) +{ + uint16_t maxBankAngle = targetPidProfile->max_angle_inclination[axis]; + if (!targetIsFixedWing && + navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI && + isAdjustingPosition()) { + maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle); + } + + float angleTarget; + +#ifdef USE_PROGRAMMING_FRAMEWORK + angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), maxBankAngle); +#else + angleTarget = pidRcCommandToAngle(rcCommand[axis], maxBankAngle); +#endif + + if (targetIsFixedWing && axis == FD_PITCH) { +#ifdef USE_FW_AUTOLAND + if (FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { +#else + if (FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { +#endif + angleTarget += scaleRange( + MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), + 0, + currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, + 0, + navConfig()->fw.minThrottleDownPitchAngle + ); + } + + angleTarget -= DEGREES_TO_DECIDEGREES(targetPidProfile->fixedWingLevelTrim); + } + + return angleTarget; +} + +static void pidAutoTransitionTargetLevel( + const float angleTarget, + autoTransitionTargetPidState_t *pidState, + const controlConfig_t *controlProfile, + const pidProfile_t *targetPidProfile, + const bool targetIsFixedWing, + flight_dynamics_index_t axis, + float horizonRateMagnitude, + float dT) +{ + float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); + const pidBank_t *targetPidBank = targetIsFixedWing ? &targetPidProfile->bank_fw : &targetPidProfile->bank_mc; + + if (targetIsFixedWing && FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) { + angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)); + if (!angleErrorDeg) { + pidState->errorGyroIf = 0.0f; + pidState->errorGyroIfLimit = 0.0f; + } + } + + float angleRateTarget = constrainf( + angleErrorDeg * (targetPidBank->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), + -controlProfile->stabilized.rates[axis] * 10.0f, + controlProfile->stabilized.rates[axis] * 10.0f + ); + + if (targetPidBank->pid[PID_LEVEL].I) { + angleRateTarget = pt1FilterApply3(&pidState->angleFilterState, angleRateTarget, dT); + } + + if (FLIGHT_MODE(HORIZON_MODE)) { + pidState->rateTarget = (1.0f - horizonRateMagnitude) * angleRateTarget + horizonRateMagnitude * pidState->rateTarget; + } else { + pidState->rateTarget = angleRateTarget; + } +} + +static void pidApplyAutoTransitionTargetSetpointRateLimiting( + autoTransitionTargetPidState_t *pidState, + const pidProfile_t *targetPidProfile, + flight_dynamics_index_t axis, + float dT) +{ + const uint32_t axisAccelLimit = (axis == FD_YAW) ? targetPidProfile->axisAccelerationLimitYaw : targetPidProfile->axisAccelerationLimitRollPitch; + + if (axisAccelLimit > AXIS_ACCEL_MIN_LIMIT) { + pidState->rateTarget = rateLimitFilterApply4(&pidState->axisAccelFilter, pidState->rateTarget, (float)axisAccelLimit, dT); + } +} + +static void pidAutoTransitionTargetTurnAssistant( + autoTransitionTargetPidState_t *pidState, + const controlConfig_t *controlProfile, + const pidProfile_t *targetPidProfile, + const bool targetIsFixedWing, + float bankAngleTarget, + float pitchAngleTarget) +{ + fpVector3_t targetRates; + targetRates.x = 0.0f; + targetRates.y = 0.0f; + + if (targetIsFixedWing) { + if (calculateCosTiltAngle() < 0.173648f) { + return; + } + +#if defined(USE_PITOT) + float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) && pitotIsHealthy() ? getAirspeedEstimate() : targetPidProfile->fixedWingReferenceAirspeed; +#else + float airspeedForCoordinatedTurn = targetPidProfile->fixedWingReferenceAirspeed; +#endif + + airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300.0f, 6000.0f); + bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60)); + + const float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget)); + const float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor; + + targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); + } else { + targetRates.z = pidState[YAW].rateTarget; + } + + imuTransformVectorEarthToBody(&targetRates); + + pidState[ROLL].rateTarget = constrainf( + pidState[ROLL].rateTarget + targetRates.x, + -controlProfile->stabilized.rates[ROLL] * 10.0f, + controlProfile->stabilized.rates[ROLL] * 10.0f + ); + pidState[PITCH].rateTarget = constrainf( + pidState[PITCH].rateTarget + targetRates.y * targetPidProfile->fixedWingCoordinatedPitchGain, + -controlProfile->stabilized.rates[PITCH] * 10.0f, + controlProfile->stabilized.rates[PITCH] * 10.0f + ); + + if (targetIsFixedWing) { + pidState[YAW].rateTarget = constrainf( + pidState[YAW].rateTarget + targetRates.z * targetPidProfile->fixedWingCoordinatedYawGain, + -controlProfile->stabilized.rates[YAW] * 10.0f, + controlProfile->stabilized.rates[YAW] * 10.0f + ); + } else { + pidState[YAW].rateTarget = constrainf( + targetRates.z, + -controlProfile->stabilized.rates[YAW] * 10.0f, + controlProfile->stabilized.rates[YAW] * 10.0f + ); + } +} + +static void updateAutoTransitionTargetRateTargets(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, const bool targetIsFixedWing, float dT) +{ + const uint8_t headingHoldState = getAutoTransitionTargetHeadingHoldState(targetPidProfile); + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + float rateTarget; + + if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) { + rateTarget = pidAutoTransitionTargetHeadingHold(targetPidProfile, targetIsFixedWing, dT); + } else { +#ifdef USE_PROGRAMMING_FRAMEWORK + rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), controlProfile->stabilized.rates[axis]); +#else + rateTarget = pidRcCommandToRate(rcCommand[axis], controlProfile->stabilized.rates[axis]); +#endif + } + + autoTransitionTargetPidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + } + + const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcAutoTransitionTargetHorizonRateMagnitude(targetPidProfile, targetIsFixedWing) : 0.0f; + + for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) { + float angleTarget = getFlightAxisAngleOverride(axis, computeAutoTransitionTargetPidLevelTarget(targetPidProfile, targetIsFixedWing, axis)); + + if (STATE(TAILSITTER) && isMixerTransitionMixing && axis == FD_PITCH) { + angleTarget += DEGREES_TO_DECIDEGREES(45); + } + + // Preview the target profile angle controller using the current + // transition stick command. Angle-hold target memory is not + // duplicated here, so this remains a close preview rather than a + // bit-for-bit copy of the post-switch controller state. + pidAutoTransitionTargetLevel(angleTarget, &autoTransitionTargetPidState[axis], controlProfile, targetPidProfile, targetIsFixedWing, axis, horizonRateMagnitude, dT); + } + } + + if (FLIGHT_MODE(TURN_ASSISTANT) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + const float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], targetPidProfile->max_angle_inclination[FD_ROLL])); + const float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], targetPidProfile->max_angle_inclination[FD_PITCH])); + pidAutoTransitionTargetTurnAssistant(autoTransitionTargetPidState, controlProfile, targetPidProfile, targetIsFixedWing, bankAngleTarget, pitchAngleTarget); + } + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + pidApplyAutoTransitionTargetSetpointRateLimiting(&autoTransitionTargetPidState[axis], targetPidProfile, axis, dT); + } +} + +static float autoTransitionTargetPTermProcess(autoTransitionTargetPidState_t *pidState, flight_dynamics_index_t axis, float rateError, float dT) +{ + const float newPTerm = rateError * pidState->kP; + UNUSED(axis); + + return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, dT); +} + +#ifdef USE_D_BOOST +static float applyAutoTransitionTargetDBoost(autoTransitionTargetPidState_t *pidState, const pidProfile_t *targetPidProfile, float currentRateTarget, float dT, float dT_inv) +{ + float dBoost = 1.0f; + const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv; + const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); + const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv); + + if (dBoostGyroAcceleration >= dBoostRateAcceleration) { + dBoost = scaleRangef(dBoostGyroAcceleration, 0.0f, targetPidProfile->dBoostMaxAtAlleceleration, 1.0f, targetPidProfile->dBoostMax); + } else { + dBoost = scaleRangef(dBoostRateAcceleration, 0.0f, pidState->dBoostTargetAcceleration, 1.0f, targetPidProfile->dBoostMin); + } + + dBoost = pt1FilterApply3(&pidState->dBoostLpf, dBoost, dT); + return constrainf(dBoost, targetPidProfile->dBoostMin, targetPidProfile->dBoostMax); +} +#else +static float applyAutoTransitionTargetDBoost(autoTransitionTargetPidState_t *pidState, const pidProfile_t *targetPidProfile, float currentRateTarget, float dT, float dT_inv) +{ + UNUSED(pidState); + UNUSED(targetPidProfile); + UNUSED(currentRateTarget); + UNUSED(dT); + UNUSED(dT_inv); + return 1.0f; +} +#endif + +static float autoTransitionTargetDTermProcess(autoTransitionTargetPidState_t *pidState, const pidProfile_t *targetPidProfile, float currentRateTarget, float dT, float dT_inv) +{ + if (pidState->kD == 0.0f) { + return 0.0f; + } + + float delta = pidState->previousRateGyro - pidState->gyroRate; + delta = autoTransitionTargetDTermLpfFilterApplyFn((filter_t *)&pidState->dtermLpfState, delta); + + return delta * (pidState->kD * dT_inv) * applyAutoTransitionTargetDBoost(pidState, targetPidProfile, currentRateTarget, dT, dT_inv); +} + +static void applyAutoTransitionTargetItermLock(autoTransitionTargetPidState_t *pidState, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis, const float rateTarget, const float rateError) +{ + const float maxRate = controlProfile->stabilized.rates[axis] * 10.0f; + const float dampingFactor = attenuation(rateTarget, maxRate * targetPidProfile->fwItermLockRateLimit / 100.0f); + const bool errorThresholdReached = fabsf(rateError) > maxRate * targetPidProfile->fwItermLockEngageThreshold / 100.0f; + + if (fabsf(rateTarget) > maxRate * 0.2f) { + pidState->attenuation.targetOverThresholdTimeMs = pidLoopNowMs; + } + + if (!errorThresholdReached) { + pidState->attenuation.targetOverThresholdTimeMs = 0; + } + + pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (pidLoopNowMs - pidState->attenuation.targetOverThresholdTimeMs) < targetPidProfile->fwItermLockTimeMaxMs) ? 0.0f : 1.0f); + pidState->attenuation.aP = dampingFactor; + pidState->attenuation.aD = dampingFactor; +} + +static bool checkAutoTransitionTargetItermLimitingActive(void) +{ + return STATE(ANTI_WINDUP); +} + +static void applyAutoTransitionTargetItermLimiting(autoTransitionTargetPidState_t *pidState, const bool itermLimitActive) +{ + if (itermLimitActive) { + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); + } else { + pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); + } +} + +static bool checkAutoTransitionTargetItermFreezingActive(const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis) +{ + if (targetPidProfile->fixedWingYawItermBankFreeze != 0 && + axis == FD_YAW) { + const float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); + return fabsf(bankAngle) > targetPidProfile->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT)); + } + + return false; +} + +static int16_t applyAutoTransitionTargetFixedWingRateController(autoTransitionTargetPidState_t *pidState, const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis, const bool itermLimitActive, const bool itermFreezeActive, float dT, float dT_inv) +{ + const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); + const float rateError = rateTarget - pidState->gyroRate; + + applyAutoTransitionTargetItermLock(pidState, controlProfile, targetPidProfile, axis, rateTarget, rateError); + + const float newPTerm = autoTransitionTargetPTermProcess(pidState, axis, rateError, dT) * pidState->attenuation.aP; + const float newDTerm = autoTransitionTargetDTermProcess(pidState, targetPidProfile, rateTarget, dT, dT_inv) * pidState->attenuation.aD; + const float newFFTerm = rateTarget * pidState->kFF; + + if (!itermFreezeActive) { + pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI; + } + + applyAutoTransitionTargetItermLimiting(pidState, itermLimitActive); + + const uint16_t limit = 500; + + if (targetPidProfile->pidItermLimitPercent != 0) { + const float itermLimit = limit * targetPidProfile->pidItermLimitPercent * 0.01f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); + } + + pidState->previousRateGyro = pidState->gyroRate; + pidState->previousRateTarget = rateTarget; + + return constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -limit, +limit); +} + +static int16_t applyAutoTransitionTargetMulticopterRateController(autoTransitionTargetPidState_t *pidState, const pidProfile_t *targetPidProfile, flight_dynamics_index_t axis, const bool itermLimitActive, float dT, float dT_inv) +{ + const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); + const float rateError = rateTarget - pidState->gyroRate; + const float newPTerm = autoTransitionTargetPTermProcess(pidState, axis, rateError, dT); + const float newDTerm = autoTransitionTargetDTermProcess(pidState, targetPidProfile, rateTarget, dT, dT_inv); + + const float rateTargetDelta = rateTarget - pidState->previousRateTarget; + const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta); + const float newCDTerm = rateTargetDeltaFiltered * pidState->kCD; + + const uint16_t limit = 500; + const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; + const float newOutputLimited = constrainf(newOutput, -limit, +limit); + + /* + * Keep this preview independent from the active controller state. Reusing + * the global I-term relax filters here would make target-profile preview + * change the current-profile controller history during transition. + */ + pidState->errorGyroIf += (rateError * pidState->kI * antiWindupScaler * dT) + + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + + if (targetPidProfile->pidItermLimitPercent != 0) { + const float itermLimit = limit * targetPidProfile->pidItermLimitPercent * 0.01f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); + } + + applyAutoTransitionTargetItermLimiting(pidState, itermLimitActive); + + pidState->previousRateTarget = rateTarget; + pidState->previousRateGyro = pidState->gyroRate; + + return newOutputLimited; +} + +static void NOINLINE updateAutoTransitionTargetAxisPID(float dT) +{ + if (!pidFiltersConfigured || !isAutoTransitionTargetPidActive()) { + resetAutoTransitionTargetPidState(); + return; + } + + const uint8_t profileIndex = getAutoTransitionTargetControlProfileIndex(); + const controlConfig_t *controlProfile = getAutoTransitionTargetControlProfile(profileIndex); + const pidProfile_t *targetPidProfile = getAutoTransitionTargetPidProfile(profileIndex); + const bool targetIsFixedWing = isAutoTransitionTargetFixedWing(); + + if (autoTransitionTargetControlProfileIndex != profileIndex || + autoTransitionTargetFixedWing != targetIsFixedWing) { + initAutoTransitionTargetPidState(profileIndex, controlProfile, targetPidProfile, targetIsFixedWing); + } + + updateAutoTransitionTargetPIDCoefficients(controlProfile, targetPidProfile, targetIsFixedWing); + updateAutoTransitionTargetRateTargets(controlProfile, targetPidProfile, targetIsFixedWing, dT); + + const float dT_inv = 1.0f / dT; + const bool itermLimitActive = checkAutoTransitionTargetItermLimitingActive(); + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + autoTransitionTargetPidState[axis].gyroRate = gyro.gyroADCf[axis]; + +#ifdef USE_SMITH_PREDICTOR + autoTransitionTargetPidState[axis].gyroRate = applySmithPredictor(axis, &autoTransitionTargetPidState[axis].smithPredictor, autoTransitionTargetPidState[axis].gyroRate); +#endif + + if (targetIsFixedWing) { + const bool itermFreezeActive = checkAutoTransitionTargetItermFreezingActive(targetPidProfile, axis); + autoTransitionTargetAxisPID[axis] = applyAutoTransitionTargetFixedWingRateController(&autoTransitionTargetPidState[axis], controlProfile, targetPidProfile, axis, itermLimitActive, itermFreezeActive, dT, dT_inv); + } else { + autoTransitionTargetAxisPID[axis] = applyAutoTransitionTargetMulticopterRateController(&autoTransitionTargetPidState[axis], targetPidProfile, axis, itermLimitActive, dT, dT_inv); + } + } +} + +int16_t NOINLINE getAutoTransitionTargetAxisPID(flight_dynamics_index_t axis) +{ + if (!isAutoTransitionTargetPidActive()) { + return 0; + } + + return autoTransitionTargetAxisPID[axis]; +} + +int16_t getAutoTransitionTargetStabilizedInput(flight_dynamics_index_t axis) +{ + if (!(isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction != MIXERAT_DIRECTION_NONE)) { + return 0; + } + + const float scale = currentMixerConfig.vtolTransitionDynamicMixer ? mixerATGetFwAuthorityScale() : 1.0f; + + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + if (FLIGHT_MODE(MANUAL_MODE)) { + return lrintf(rcCommand[axis] * scale); + } + + return lrintf(autoTransitionTargetAxisPID[axis] * scale); + } + + // FW->MC does not preview multirotor lift stabilisation through these + // inputs. Returning current FW axisPID here would incorrectly feed fixed- + // wing controller output into rules that users may expect to represent MC + // takeover. + return 0; +} +#endif + static float FAST_CODE pidRcCommandToAngle(int16_t stick, int16_t maxInclination) { stick = constrain(stick, -500, 500); @@ -1263,6 +2057,9 @@ void FAST_CODE pidController(float dT) pidLoopNowMs = millis(); if (!pidFiltersConfigured) { +#ifdef USE_AUTO_TRANSITION + resetAutoTransitionTargetPidState(); +#endif return; } @@ -1363,6 +2160,10 @@ void FAST_CODE pidController(float dT) pidControllerApplyFn(&pidState[axis], dT, dT_inv); } + +#ifdef USE_AUTO_TRANSITION + updateAutoTransitionTargetAxisPID(dT); +#endif } pidType_e pidIndexGetType(pidIndex_e pidIndex) @@ -1462,6 +2263,10 @@ void pidInit(void) 0.0f ); +#ifdef USE_AUTO_TRANSITION + resetAutoTransitionTargetPidState(); +#endif + } const pidBank_t * pidBank(void) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cb428140cf6..4c615c782ae 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -189,7 +189,14 @@ const pidBank_t * pidBank(void); pidBank_t * pidBankMutable(void); extern int16_t axisPID[]; +#ifdef USE_AUTO_TRANSITION +extern int16_t autoTransitionTargetAxisPID[]; +#endif extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_F[], axisPID_Setpoint[]; +#ifdef USE_AUTO_TRANSITION +int16_t getAutoTransitionTargetStabilizedInput(flight_dynamics_index_t axis); +int16_t getAutoTransitionTargetAxisPID(flight_dynamics_index_t axis); +#endif void pidInit(void); bool pidInitFilters(void); @@ -229,4 +236,4 @@ bool isFixedWingLevelTrimActive(void); void updateFixedWingLevelTrim(timeUs_t currentTimeUs); float getFixedWingLevelTrim(void); bool isAngleHoldLevel(void); -uint16_t getPidSumLimit(const flight_dynamics_index_t axis); \ No newline at end of file +uint16_t getPidSumLimit(const flight_dynamics_index_t axis); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 7b62fd1059a..1c7c65b3899 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -51,6 +51,8 @@ #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" +#include "flight/mixer_transition_logic.h" #include "flight/pid.h" #include "flight/servos.h" @@ -106,6 +108,14 @@ int16_t servo[MAX_SUPPORTED_SERVOS]; static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; +#ifdef USE_AUTO_TRANSITION +static uint32_t targetTransitionServoPreviewCaptureMask = 0; +static int targetTransitionServoPreviewTargetProfileIndex = -1; +static mixerProfileATDirection_e targetTransitionServoPreviewDirection = MIXERAT_DIRECTION_NONE; +static int16_t targetTransitionServoPreviewCapturedOutput[MAX_SUPPORTED_SERVOS]; +static int16_t vtolTransitionServoDebugPreHandoff[MAX_SUPPORTED_SERVOS]; +static int16_t vtolTransitionServoDebugPostHandoff[MAX_SUPPORTED_SERVOS]; +#endif /* //Was used to keep track of servo rules in all mixer_profile, In order to Apply mixer speed limit when rules turn off static servoMixer_t currentServoMixer[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; @@ -123,6 +133,7 @@ static bool servoFilterIsSet; static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS]; static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES]; +static bool carryOverServoSpeedLimitsOnNextLoad = true; STATIC_FASTRAM pt1Filter_t rotRateFilter; STATIC_FASTRAM pt1Filter_t targetRateFilter; @@ -215,19 +226,25 @@ void loadCustomServoMixer(void) int movefilterCount = 0; static servoMixerSwitch_t servoMixerSwitchHelper[MAX_SERVO_RULES_SWITCH_CARRY]; // helper to keep track of servoSpeedLimitFilter of servo rules memset(servoMixerSwitchHelper, 0, sizeof(servoMixerSwitchHelper)); - for (int i = 0; i < servoRuleCount; i++) { - if(currentServoMixer[i].inputSource == INPUT_MIXER_SWITCH_HELPER || movefilterCount >= MAX_SERVO_RULES_SWITCH_CARRY) { - //will not carry over INPUT_MIXER_SWITCH_HELPER rules - break; - } - if(currentServoMixer[i].speed != 0 && fabsf(servoSpeedLimitFilter[i].state) > 0.01f) { - servoMixerSwitchHelper[movefilterCount].targetChannel = currentServoMixer[i].targetChannel; - servoMixerSwitchHelper[movefilterCount].speed = currentServoMixer[i].speed; - servoMixerSwitchHelper[movefilterCount].rate = currentServoMixer[i].rate; - servoMixerSwitchHelper[movefilterCount].speedLimitFilterState = servoSpeedLimitFilter[i].state; - movefilterCount++; + // Profile hot-switches normally preserve speed-limited rule outputs so old + // rules can decay smoothly. Direct switches can opt out for an immediate + // target-profile response. + if (carryOverServoSpeedLimitsOnNextLoad) { + for (int i = 0; i < servoRuleCount; i++) { + if(currentServoMixer[i].inputSource == INPUT_MIXER_SWITCH_HELPER || movefilterCount >= MAX_SERVO_RULES_SWITCH_CARRY) { + //will not carry over INPUT_MIXER_SWITCH_HELPER rules + break; + } + if(currentServoMixer[i].speed != 0 && fabsf(servoSpeedLimitFilter[i].state) > 0.01f) { + servoMixerSwitchHelper[movefilterCount].targetChannel = currentServoMixer[i].targetChannel; + servoMixerSwitchHelper[movefilterCount].speed = currentServoMixer[i].speed; + servoMixerSwitchHelper[movefilterCount].rate = currentServoMixer[i].rate; + servoMixerSwitchHelper[movefilterCount].speedLimitFilterState = servoSpeedLimitFilter[i].state; + movefilterCount++; + } } } + carryOverServoSpeedLimitsOnNextLoad = true; servoRuleCount = 0; memset(currentServoMixer, 0, sizeof(currentServoMixer)); @@ -257,6 +274,11 @@ void loadCustomServoMixer(void) } } +void servoMixerSetCarryoverOnNextLoad(bool enabled) +{ + carryOverServoSpeedLimitsOnNextLoad = enabled; +} + static void filterServos(void) { if (servoConfig()->servo_lowpass_freq) { @@ -280,6 +302,303 @@ static void filterServos(void) } } +#ifdef USE_AUTO_TRANSITION +static bool isAutoTransitionTargetInputSource(const uint8_t inputSource) +{ + return inputSource >= INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL && + inputSource <= INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS; +} + +static uint16_t packServoDebugOutput(int16_t output) +{ + return constrain(output - 900, 0, 0x7FF); +} + +static uint32_t packVtolTransitionServoDebug(const uint8_t servoIndex) +{ + uint8_t flags = 0; + + if (vtolTransitionServoDebugPreHandoff[servoIndex] != vtolTransitionServoDebugPostHandoff[servoIndex]) { + flags |= 1 << 0; + } + + if ((mixerProfileAT.servoHandoffMask & (1U << servoIndex)) != 0) { + flags |= 1 << 1; + } + + if ((targetTransitionServoPreviewCaptureMask & (1U << servoIndex)) != 0) { + flags |= 1 << 2; + } + + return ((uint32_t)servoIndex & 0xFU) | + ((uint32_t)packServoDebugOutput(vtolTransitionServoDebugPreHandoff[servoIndex]) << 4) | + ((uint32_t)packServoDebugOutput(vtolTransitionServoDebugPostHandoff[servoIndex]) << 15) | + ((uint32_t)flags << 26); +} + +static int16_t vtolTransitionServoDebugActivity(const uint8_t servoIndex) +{ + const int16_t middle = servoParams(servoIndex)->middle; + const int16_t preDelta = ABS(vtolTransitionServoDebugPreHandoff[servoIndex] - middle); + const int16_t postDelta = ABS(vtolTransitionServoDebugPostHandoff[servoIndex] - middle); + const int16_t handoffDelta = ABS(vtolTransitionServoDebugPostHandoff[servoIndex] - vtolTransitionServoDebugPreHandoff[servoIndex]); + + return MAX(MAX(preDelta, postDelta), handoffDelta); +} + +int32_t servoMixerGetVtolTransitionDebug(const uint8_t slot) +{ + uint8_t bestIndex[2] = { 0, 0 }; + int16_t bestActivity[2] = { -1, -1 }; + + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + const int16_t activity = vtolTransitionServoDebugActivity(i); + + if (activity > bestActivity[0]) { + bestActivity[1] = bestActivity[0]; + bestIndex[1] = bestIndex[0]; + bestActivity[0] = activity; + bestIndex[0] = i; + } else if (activity > bestActivity[1]) { + bestActivity[1] = activity; + bestIndex[1] = i; + } + } + + return (int32_t)packVtolTransitionServoDebug(bestIndex[slot > 0 ? 1 : 0]); +} + +static bool isTransitionServoPreviewInputSource(const uint8_t inputSource) +{ + return inputSource == INPUT_MAX || + inputSource == INPUT_MIXER_TRANSITION; +} + +static bool servoRuleConditionIsActive(const servoMixer_t *rule) +{ +#ifdef USE_PROGRAMMING_FRAMEWORK + return logicConditionGetValue(rule->conditionId); +#else + UNUSED(rule); + return true; +#endif +} + +static bool servoProfileHasTargetInputSource(const servoMixer_t *rules, const uint8_t targetChannel, const uint8_t inputSource) +{ + for (int i = 0; i < MAX_SERVO_RULES; i++) { + const servoMixer_t *rule = &rules[i]; + + if (rule->rate == 0) { + break; + } + + if (rule->targetChannel == targetChannel && + rule->inputSource == inputSource && + servoRuleConditionIsActive(rule)) { + return true; + } + } + + return false; +} + +static void resetTargetTransitionServoPreviewCapture(void) +{ + targetTransitionServoPreviewCaptureMask = 0; + targetTransitionServoPreviewTargetProfileIndex = -1; + targetTransitionServoPreviewDirection = MIXERAT_DIRECTION_NONE; + memset(targetTransitionServoPreviewCapturedOutput, 0, sizeof(targetTransitionServoPreviewCapturedOutput)); +} + +static void updateTargetTransitionServoPreviewSession(void) +{ + if (!isMixerTransitionMixing || + !mixerATIsActive() || + mixerProfileAT.phase != MIXERAT_PHASE_TRANSITIONING || + nextMixerProfileIndex < 0 || + nextMixerProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + resetTargetTransitionServoPreviewCapture(); + return; + } + + if (targetTransitionServoPreviewTargetProfileIndex != nextMixerProfileIndex || + targetTransitionServoPreviewDirection != mixerProfileAT.direction) { + resetTargetTransitionServoPreviewCapture(); + targetTransitionServoPreviewTargetProfileIndex = nextMixerProfileIndex; + targetTransitionServoPreviewDirection = mixerProfileAT.direction; + } +} + +static bool servoNeedsTargetTransitionPreview(const uint8_t targetChannel) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer || + currentMixerConfig.vtolTransitionScaleRampTimeMs == 0 || + !isMixerTransitionMixing || + !mixerATIsActive() || + mixerProfileAT.phase != MIXERAT_PHASE_TRANSITIONING || + mixerProfileAT.hotSwitchDone || + mixerProfileAT.direction != MIXERAT_DIRECTION_TO_MC || + nextMixerProfileIndex < 0 || + nextMixerProfileIndex >= MAX_MIXER_PROFILE_COUNT || + !isMultirotorTypePlatform(mixerConfigByIndex(nextMixerProfileIndex)->platformType) || + targetChannel >= MAX_SUPPORTED_SERVOS) { + return false; + } + + const servoMixer_t *currentRules = mixerServoMixersByIndex(currentMixerProfileIndex); + const servoMixer_t *targetRules = mixerServoMixersByIndex(nextMixerProfileIndex); + + return !servoProfileHasTargetInputSource(currentRules, targetChannel, INPUT_MIXER_TRANSITION) && + servoProfileHasTargetInputSource(targetRules, targetChannel, INPUT_MIXER_TRANSITION); +} + +static int16_t applyServoOutputScaling(const uint8_t servoIndex, int32_t servoOutput) +{ + servoOutput = ((int32_t)servoParams(servoIndex)->rate * servoOutput) / 100L; + + if (servoOutput > 0) { + servoOutput = (int16_t)(servoOutput * servoMetadata[servoIndex].scaleMax); + } else { + servoOutput = (int16_t)(servoOutput * servoMetadata[servoIndex].scaleMin); + } + + servoOutput += servoParams(servoIndex)->middle; + return constrain(servoOutput, servoParams(servoIndex)->min, servoParams(servoIndex)->max); +} + +static bool getTargetTransitionServoPreviewOutput(const uint8_t targetChannel, const int16_t input[INPUT_SOURCE_COUNT], int16_t *output) +{ + if (nextMixerProfileIndex < 0 || + nextMixerProfileIndex >= MAX_MIXER_PROFILE_COUNT || + targetChannel >= MAX_SUPPORTED_SERVOS) { + return false; + } + + const servoMixer_t *targetRules = mixerServoMixersByIndex(nextMixerProfileIndex); + bool hasPreviewRule = false; + int32_t servoOutput = 0; + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + const servoMixer_t *rule = &targetRules[i]; + + if (rule->rate == 0) { + break; + } + + if (rule->targetChannel != targetChannel || + !isTransitionServoPreviewInputSource(rule->inputSource) || + !servoRuleConditionIsActive(rule)) { + continue; + } + + hasPreviewRule = true; + servoOutput += ((int32_t)input[rule->inputSource] * rule->rate) / 100; + } + + if (!hasPreviewRule) { + return false; + } + + *output = applyServoOutputScaling(targetChannel, servoOutput); + return true; +} + +static float getTargetTransitionServoPreviewProgress(const int16_t transitionServoInput) +{ + return constrainf((500.0f - transitionServoInput) / 500.0f, 0.0f, 1.0f); +} + +static bool getTargetTransitionServoPreviewMixedOutput(const uint8_t targetChannel, const int16_t input[INPUT_SOURCE_COUNT], const int16_t currentOutput, int16_t *output) +{ + if (!servoNeedsTargetTransitionPreview(targetChannel)) { + return false; + } + + const uint32_t targetMask = 1U << targetChannel; + if ((targetTransitionServoPreviewCaptureMask & targetMask) == 0) { + targetTransitionServoPreviewCapturedOutput[targetChannel] = currentOutput; + targetTransitionServoPreviewCaptureMask |= targetMask; + } + + int16_t targetOutput = 0; + if (!getTargetTransitionServoPreviewOutput(targetChannel, input, &targetOutput)) { + return false; + } + + *output = mixerTransitionBlendCapturedServoOutput( + targetTransitionServoPreviewCapturedOutput[targetChannel], + targetOutput, + getTargetTransitionServoPreviewProgress(input[INPUT_MIXER_TRANSITION])); + return true; +} + +static bool isStabilizedAxisInputSource(const uint8_t inputSource) +{ + switch (inputSource) { + case INPUT_STABILIZED_ROLL: + case INPUT_STABILIZED_PITCH: + case INPUT_STABILIZED_YAW: + case INPUT_STABILIZED_ROLL_PLUS: + case INPUT_STABILIZED_ROLL_MINUS: + case INPUT_STABILIZED_PITCH_PLUS: + case INPUT_STABILIZED_PITCH_MINUS: + case INPUT_STABILIZED_YAW_PLUS: + case INPUT_STABILIZED_YAW_MINUS: + return true; + default: + return false; + } +} + +static int getAutoTransitionServoProfileIndex(void) +{ + if (isMultirotorTypePlatform(currentMixerConfig.platformType)) { + return currentMixerProfileIndex; + } + + if (nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT && + isMultirotorTypePlatform(mixerConfigByIndex(nextMixerProfileIndex)->platformType)) { + return nextMixerProfileIndex; + } + + return -1; +} + +static void collectAutoTransitionServoTargets(bool targets[MAX_SUPPORTED_SERVOS]) +{ + memset(targets, 0, MAX_SUPPORTED_SERVOS * sizeof(targets[0])); + + const int profileIndex = getAutoTransitionServoProfileIndex(); + if (profileIndex < 0) { + return; + } + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + const servoMixer_t *rule = &mixerServoMixersByIndex(profileIndex)[i]; + + if (rule->rate == 0) { + break; + } + + if (!isAutoTransitionTargetInputSource(rule->inputSource)) { + continue; + } + +#ifdef USE_PROGRAMMING_FRAMEWORK + if (!logicConditionGetValue(rule->conditionId)) { + continue; + } +#endif + + if (rule->targetChannel < MAX_SUPPORTED_SERVOS) { + targets[rule->targetChannel] = true; + } + } +} +#endif + void writeServos(void) { filterServos(); @@ -308,6 +627,24 @@ void writeServos(void) void servoMixer(float dT) { int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] +#ifdef USE_AUTO_TRANSITION + const bool autoTransitionInputsActive = isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction != MIXERAT_DIRECTION_NONE; + const bool autoTransitionTargetPreviewActive = autoTransitionInputsActive && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW; + const bool scaleCurrentFwServoRules = autoTransitionInputsActive && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC && + !isMultirotorTypePlatform(currentMixerConfig.platformType); + const float currentFwServoScale = currentMixerConfig.vtolTransitionDynamicMixer ? mixerATGetFwAuthorityScale() : 1.0f; + bool autoTransitionServoTargets[MAX_SUPPORTED_SERVOS]; + + if (scaleCurrentFwServoRules) { + collectAutoTransitionServoTargets(autoTransitionServoTargets); + } else { + memset(autoTransitionServoTargets, 0, sizeof(autoTransitionServoTargets)); + } +#endif if (FLIGHT_MODE(MANUAL_MODE)) { input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; @@ -326,12 +663,30 @@ void servoMixer(float dT) } } +#ifdef USE_AUTO_TRANSITION + // These preview inputs are only for pre-switch FW servo handoff. + // In FW->MC the same marked rules are used only as identifiers so the + // currently active FW servo rules can fade out; they do not become MC + // lift-motor stabilisation sources. + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL] = autoTransitionTargetPreviewActive ? getAutoTransitionTargetStabilizedInput(FD_ROLL) : 0; + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH] = autoTransitionTargetPreviewActive ? getAutoTransitionTargetStabilizedInput(FD_PITCH) : 0; + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW] = autoTransitionTargetPreviewActive ? getAutoTransitionTargetStabilizedInput(FD_YAW) : 0; +#endif + input[INPUT_STABILIZED_ROLL_PLUS] = constrain(input[INPUT_STABILIZED_ROLL], 0, 1000); input[INPUT_STABILIZED_ROLL_MINUS] = constrain(input[INPUT_STABILIZED_ROLL], -1000, 0); input[INPUT_STABILIZED_PITCH_PLUS] = constrain(input[INPUT_STABILIZED_PITCH], 0, 1000); input[INPUT_STABILIZED_PITCH_MINUS] = constrain(input[INPUT_STABILIZED_PITCH], -1000, 0); input[INPUT_STABILIZED_YAW_PLUS] = constrain(input[INPUT_STABILIZED_YAW], 0, 1000); input[INPUT_STABILIZED_YAW_MINUS] = constrain(input[INPUT_STABILIZED_YAW], -1000, 0); +#ifdef USE_AUTO_TRANSITION + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_PLUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL], 0, 1000); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_MINUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL], -1000, 0); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_PLUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH], 0, 1000); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_MINUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH], -1000, 0); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_PLUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW], 0, 1000); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS] = constrain(input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW], -1000, 0); +#endif input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0; @@ -357,8 +712,11 @@ 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] = mixerATGetTransitionServoInput(); input[INPUT_MIXER_SWITCH_HELPER] = 0; // no input, used to apply speed limit filter from previous servo rules +#ifdef USE_AUTO_TRANSITION + updateTargetTransitionServoPreviewSession(); +#endif // center the RC input value around the RC middle value // by subtracting the RC middle value from the RC input value, we get: @@ -458,6 +816,15 @@ void servoMixer(float dT) inputRaw = 0; } #endif + +#ifdef USE_AUTO_TRANSITION + if (scaleCurrentFwServoRules && + autoTransitionServoTargets[target] && + isStabilizedAxisInputSource(from)) { + inputRaw = lrintf(inputRaw * currentFwServoScale); + } +#endif + /* * Apply mixer speed limit. 1 [one] speed unit is defined as 10us/s: * 0 = no limiting @@ -499,6 +866,22 @@ void servoMixer(float dT) * allowed the situation when smix weight sum for an output was above 100 */ servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max); + +#ifdef USE_AUTO_TRANSITION + vtolTransitionServoDebugPreHandoff[i] = servo[i]; + + int16_t targetPreviewOutput = 0; + if (getTargetTransitionServoPreviewMixedOutput(i, input, servo[i], &targetPreviewOutput)) { + servo[i] = constrain(targetPreviewOutput, servoParams(i)->min, servoParams(i)->max); + } + + int16_t handoffOutput = 0; + if (mixerATGetServoHandoffOutput(i, servo[i], &handoffOutput)) { + servo[i] = constrain(handoffOutput, servoParams(i)->min, servoParams(i)->max); + } + + vtolTransitionServoDebugPostHandoff[i] = servo[i]; +#endif } } diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index aecd78fe319..32ba060bbaa 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -85,6 +85,17 @@ typedef enum { INPUT_RC_CH33 = 58, INPUT_RC_CH34 = 59, INPUT_MIXER_SWITCH_HELPER = 60, +#ifdef USE_AUTO_TRANSITION + INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL = 61, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH = 62, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW = 63, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_PLUS = 64, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_MINUS = 65, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_PLUS = 66, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_MINUS = 67, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_PLUS = 68, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS = 69, +#endif INPUT_SOURCE_COUNT } inputSource_e; @@ -195,6 +206,10 @@ void setServoOutputEnabled(bool flag); bool isMixerUsingServos(void); void writeServos(void); void loadCustomServoMixer(void); +void servoMixerSetCarryoverOnNextLoad(bool enabled); +#ifdef USE_AUTO_TRANSITION +int32_t servoMixerGetVtolTransitionDebug(uint8_t slot); +#endif void servoMixer(float dT); void servoComputeScalingFactors(uint8_t servoIndex); void servosInit(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 500140a3bbd..bbef009c450 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -6121,8 +6121,14 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter const char *failsafeInfoMessage = NULL; const char *invertedInfoMessage = NULL; + const char *vtolTransitionMessage = NULL; if (ARMING_FLAG(ARMED)) { + vtolTransitionMessage = osdVtolTransitionMessage(); + if (vtolTransitionMessage) { + ADD_MSG(vtolTransitionMessage); + } + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL NORMALLY, 5 MESSAGES DURING FAILSAFE */ if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) { @@ -6376,6 +6382,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // a lost model, but might help avoiding a crash. // Blink to grab user attention. TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (message == vtolTransitionMessage && osdVtolTransitionMessageShouldBlink()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (message == invertedInfoMessage) { TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 21af2d7b290..db70409f9dc 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -124,6 +124,30 @@ #define OSD_MSG_ANGLEHOLD_PITCH "(ANGLEHOLD PITCH)" #define OSD_MSG_ANGLEHOLD_LEVEL "(ANGLEHOLD LEVEL)" #define OSD_MSG_MOVE_STICKS "MOVE STICKS TO ABORT" +#define OSD_MSG_VTOL_TRANS_START "VTOL TRANSITION START" +#define OSD_MSG_VTOL_TO_FW "VTOL TO FW" +#define OSD_MSG_VTOL_TO_MC "VTOL TO MC" +#define OSD_MSG_VTOL_MANUAL_TO_FW "VTOL MANUAL TO FW" +#define OSD_MSG_VTOL_MANUAL_TO_MC "VTOL MANUAL TO MC" +#define OSD_MSG_VTOL_MISSION_TO_FW "VTOL MISSION TO FW" +#define OSD_MSG_VTOL_MISSION_TO_MC "VTOL MISSION TO MC" +#define OSD_MSG_VTOL_RTH_TO_FW "VTOL RTH TO FW" +#define OSD_MSG_VTOL_LAND_TO_MC "VTOL LAND TO MC" +#define OSD_MSG_VTOL_SAFE_TO_MC "VTOL LOW SPD TO MC" +#define OSD_MSG_VTOL_FINISH_SWITCH "VTOL FINISHING SWITCH" +#define OSD_MSG_VTOL_TAILSITTER_CAPTURE "VTOL TAILSITTER CAPTURE" +#define OSD_MSG_VTOL_RETRY_SCAN "VTOL RETRY SCAN" +#define OSD_MSG_VTOL_RETRY_ALIGN "VTOL RETRY ALIGN" +#define OSD_MSG_VTOL_TRANS_DONE "VTOL TRANSITION DONE" +#define OSD_MSG_VTOL_TRANS_ABORTED "VTOL TRANSITION ABORTED" +#define OSD_MSG_VTOL_ABORTED_FW "VTOL ABORTED IN FW" +#define OSD_MSG_VTOL_ABORTED_MC "VTOL ABORTED IN MC" +#define OSD_MSG_VTOL_AIRSPEED_TO "VTOL AIRSPEED TIMEOUT" +#define OSD_MSG_VTOL_MOVE_SW_FW "MOVE SWITCH TO FW" +#define OSD_MSG_VTOL_MOVE_SW_MC "MOVE SWITCH TO MC" +#define OSD_MSG_VTOL_WAIT_MC_SPEED "WAIT MC SPEED" +#define OSD_MSG_VTOL_NO_SPEED "NO SPEED" +#define OSD_MSG_VTOL_MC_SPEED_HIGH "MC SPEED HIGH" #ifdef USE_DEV_TOOLS #define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index 009a6559618..ac62940c1b2 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -37,6 +37,9 @@ #include "fc/settings.h" +#include "flight/mixer_profile.h" + +#include "io/osd.h" #include "io/osd_canvas.h" #include "io/osd_common.h" #include "io/osd_grid.h" @@ -72,6 +75,161 @@ int16_t osdGetSpeedFromSelectedSource(void) { return speed; } +#ifdef USE_AUTO_TRANSITION +static bool osdVtolTransitionBlink; + +static const char *osdVtolTransitionRequestMessage(const mixerProfileATRequest_e request, const mixerProfileATDirection_e direction) +{ + switch (request) { + case MIXERAT_REQUEST_RTH: + return OSD_MSG_VTOL_RTH_TO_FW; + + case MIXERAT_REQUEST_LAND: + return OSD_MSG_VTOL_LAND_TO_MC; + + case MIXERAT_REQUEST_FW_TO_MC_PROTECTION: + return OSD_MSG_VTOL_SAFE_TO_MC; + + case MIXERAT_REQUEST_MISSION_TO_FW: + return OSD_MSG_VTOL_MISSION_TO_FW; + + case MIXERAT_REQUEST_MISSION_TO_MC: + return OSD_MSG_VTOL_MISSION_TO_MC; + + case MIXERAT_REQUEST_MANUAL_TO_FW: + return OSD_MSG_VTOL_MANUAL_TO_FW; + + case MIXERAT_REQUEST_MANUAL_TO_MC: + return OSD_MSG_VTOL_MANUAL_TO_MC; + + default: + break; + } + + if (direction == MIXERAT_DIRECTION_TO_FW) { + return OSD_MSG_VTOL_TO_FW; + } + + if (direction == MIXERAT_DIRECTION_TO_MC) { + return OSD_MSG_VTOL_TO_MC; + } + + return NULL; +} + +static const char *osdVtolTransitionWaitReasonMessage(const mixerProfileATWaitReason_e waitReason) +{ + switch (waitReason) { + case MIXERAT_WAIT_REASON_MC_SPEED: + return OSD_MSG_VTOL_WAIT_MC_SPEED; + + case MIXERAT_WAIT_REASON_NO_SPEED: + return OSD_MSG_VTOL_NO_SPEED; + + case MIXERAT_WAIT_REASON_MC_SPEED_HIGH: + return OSD_MSG_VTOL_MC_SPEED_HIGH; + + case MIXERAT_WAIT_REASON_NONE: + default: + return NULL; + } +} +#endif + +const char *osdVtolTransitionMessage(void) +{ +#ifdef USE_AUTO_TRANSITION + osdVtolTransitionBlink = false; + + const navVtolTransitionOsdState_e navTransitionState = navigationVtolTransitionOsdState(); + switch (navTransitionState) { + case NAV_VTOL_TRANSITION_OSD_RETRY_SCAN: + return OSD_MSG_VTOL_RETRY_SCAN; + + case NAV_VTOL_TRANSITION_OSD_RETRY_ALIGN: + return OSD_MSG_VTOL_RETRY_ALIGN; + + case NAV_VTOL_TRANSITION_OSD_NONE: + default: + break; + } + + mixerProfileATOsdStatus_t status; + if (!mixerATGetOsdStatus(&status)) { + return NULL; + } + + if (status.switchReminderDirection == MIXERAT_DIRECTION_TO_FW) { + osdVtolTransitionBlink = true; + return OSD_MSG_VTOL_MOVE_SW_FW; + } + + if (status.switchReminderDirection == MIXERAT_DIRECTION_TO_MC) { + osdVtolTransitionBlink = true; + return OSD_MSG_VTOL_MOVE_SW_MC; + } + + if (!status.active) { + switch (status.event) { + case MIXERAT_OSD_EVENT_DONE: + return OSD_MSG_VTOL_TRANS_DONE; + + case MIXERAT_OSD_EVENT_AIRSPEED_TIMEOUT: + return OSD_MSG_VTOL_AIRSPEED_TO; + + case MIXERAT_OSD_EVENT_ABORTED: + return OSD_MSG_VTOL_TRANS_ABORTED; + + case MIXERAT_OSD_EVENT_ABORTED_IN_FW: + return OSD_MSG_VTOL_ABORTED_FW; + + case MIXERAT_OSD_EVENT_ABORTED_IN_MC: + return OSD_MSG_VTOL_ABORTED_MC; + + case MIXERAT_OSD_EVENT_NONE: + default: + return NULL; + } + } + + switch (status.phase) { + case MIXERAT_PHASE_TRANSITION_INITIALIZE: + return OSD_MSG_VTOL_TRANS_START; + + case MIXERAT_PHASE_TRANSITIONING: + { + const char *waitReasonMessage = osdVtolTransitionWaitReasonMessage(status.waitReason); + if (waitReasonMessage) { + return waitReasonMessage; + } + + return osdVtolTransitionRequestMessage(status.request, status.direction); + } + + case MIXERAT_PHASE_POST_SWITCH_FADE: + return OSD_MSG_VTOL_FINISH_SWITCH; + + case MIXERAT_PHASE_TAILSITTER_TO_MC_CAPTURE: + return OSD_MSG_VTOL_TAILSITTER_CAPTURE; + + case MIXERAT_PHASE_IDLE: + default: + break; + } +#endif + + return NULL; +} + +bool osdVtolTransitionMessageShouldBlink(void) +{ +#ifdef USE_AUTO_TRANSITION + return osdVtolTransitionBlink; +#else + return false; +#endif +} + #endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD) #if defined(USE_OSD) diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h index 0831a36e678..0045856e560 100644 --- a/src/main/io/osd_common.h +++ b/src/main/io/osd_common.h @@ -46,6 +46,8 @@ typedef struct { PG_DECLARE(osdCommonConfig_t, osdCommonConfig); int16_t osdGetSpeedFromSelectedSource(void); +const char *osdVtolTransitionMessage(void); +bool osdVtolTransitionMessageShouldBlink(void); #endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 3a69a0c77e4..110430e167c 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -995,81 +995,95 @@ static bool djiFormatMessages(char *buff) bool haveMessage = false; char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; if (ARMING_FLAG(ARMED)) { - // Aircraft is armed. We might have up to 6 - // messages to show. - char *messages[6]; + // Aircraft is armed. Keep this bounded because transition, navigation, + // failsafe and normal status messages can be active at the same time. + const char *messages[8]; unsigned messageCount = 0; + #define ADD_DJI_MSG(msg) do { if (messageCount < ARRAYLEN(messages)) messages[messageCount++] = (msg); } while (0) + + const char *vtolTransitionMessage = osdVtolTransitionMessage(); + if (vtolTransitionMessage) { + ADD_DJI_MSG(vtolTransitionMessage); + } if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while being armed too - char *failsafePhaseMessage = osdFailsafePhaseMessage(); - char *failsafeInfoMessage = osdFailsafeInfoMessage(); - char *navStateFSMessage = navigationStateMessage(); + const char *failsafePhaseMessage = osdFailsafePhaseMessage(); + const char *failsafeInfoMessage = osdFailsafeInfoMessage(); + const char *navStateFSMessage = navigationStateMessage(); if (failsafePhaseMessage) { - messages[messageCount++] = failsafePhaseMessage; + ADD_DJI_MSG(failsafePhaseMessage); } if (failsafeInfoMessage) { - messages[messageCount++] = failsafeInfoMessage; + ADD_DJI_MSG(failsafeInfoMessage); } if (navStateFSMessage) { - messages[messageCount++] = navStateFSMessage; + ADD_DJI_MSG(navStateFSMessage); } } else { #ifdef USE_SERIALRX_CRSF if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ && rxLinkStatistics.rfMode == 0) { - messages[messageCount++] = "CRSF LOW RF"; + ADD_DJI_MSG("CRSF LOW RF"); } #endif if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - char *navStateMessage = navigationStateMessage(); + const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { - messages[messageCount++] = navStateMessage; + ADD_DJI_MSG(navStateMessage); } } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { - messages[messageCount++] = "AUTOLAUNCH"; + ADD_DJI_MSG("AUTOLAUNCH"); } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO // when it doesn't require ANGLE mode (required only in FW // right now). If if requires ANGLE, its display is handled // by OSD_FLYMODE. - messages[messageCount++] = "(ALT HOLD)"; + ADD_DJI_MSG("(ALT HOLD)"); } if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { - messages[messageCount++] = "(AUTOTRIM)"; + ADD_DJI_MSG("(AUTOTRIM)"); } if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { - messages[messageCount++] = "(AUTOTUNE)"; + ADD_DJI_MSG("(AUTOTUNE)"); } if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)) { - messages[messageCount++] = "(AUTO LEVEL TRIM)"; + ADD_DJI_MSG("(AUTO LEVEL TRIM)"); } if (FLIGHT_MODE(HEADFREE_MODE)) { - messages[messageCount++] = "(HEADFREE)"; + ADD_DJI_MSG("(HEADFREE)"); } if (FLIGHT_MODE(MANUAL_MODE)) { - messages[messageCount++] = "(MANUAL)"; + ADD_DJI_MSG("(MANUAL)"); } if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { - messages[messageCount++] = "(LAND)"; + ADD_DJI_MSG("(LAND)"); } } } // Pick one of the available messages. Each message lasts // a second. if (messageCount > 0) { - strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]); - haveMessage = true; + const char *message = messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]; + if (message == vtolTransitionMessage && + osdVtolTransitionMessageShouldBlink() && + OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT / 2, 2) == 0) { + buff[0] = '\0'; + } else { + strcpy(buff, message); + } + haveMessage = true; } + #undef ADD_DJI_MSG } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { unsigned invalidIndex; // Check if we're unable to arm for some reason diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f285d50e15d..22feb6d3260 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -48,6 +48,7 @@ #include "flight/imu.h" #include "flight/mixer_profile.h" +#include "flight/mixer_transition_logic.h" #include "flight/pid.h" #include "flight/wind_estimator.h" @@ -56,11 +57,14 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "navigation/navigation_vtol_mc_protection.h" +#include "navigation/navigation_vtol_mission_logic.h" #include "navigation/rth_trackback.h" #include "rx/rx.h" #include "sensors/sensors.h" +#include "sensors/pitotmeter.h" #include "sensors/acceleration.h" #include "sensors/boardalignment.h" #include "sensors/battery.h" @@ -83,6 +87,16 @@ #define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec) #define FW_LAND_LOITER_ALT_TOLERANCE 150 +#ifdef USE_AUTO_TRANSITION +// One-shot MC->FW mission retry after airspeed-timeout: 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_ALTITUDE_FALLBACK_TOLERANCE_CM 100 +#endif + /*----------------------------------------------------------- * Compatibility for home position *-----------------------------------------------------------*/ @@ -119,7 +133,11 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif +#ifdef USE_AUTO_TRANSITION +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 11); +#else PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8); +#endif PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -149,6 +167,13 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter .waypoint_safe_distance = SETTING_NAV_WP_MAX_SAFE_DISTANCE_DEFAULT, // Metres - first waypoint should be closer than this +#ifdef USE_AUTO_TRANSITION + .vtol_mission_transition_user_action = SETTING_NAV_VTOL_MISSION_TRANSITION_USER_ACTION_DEFAULT, + .vtol_mission_transition_min_altitude = SETTING_NAV_VTOL_MISSION_TRANSITION_MIN_ALTITUDE_CM_DEFAULT, + .vtol_transition_retry_on_airspeed_timeout = SETTING_NAV_VTOL_TRANSITION_RETRY_ON_AIRSPEED_TIMEOUT_DEFAULT, + .vtol_transition_fail_action_mc_to_fw = SETTING_NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_DEFAULT, + .vtol_transition_fail_action_fw_to_mc = SETTING_NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_DEFAULT, +#endif #ifdef USE_MULTI_MISSION .waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT, // mission index selected from multi mission WP entry #endif @@ -195,7 +220,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT, // Disable boost at 1m/s .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle #endif - .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100 .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100 .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT, @@ -274,6 +298,62 @@ uint16_t navEPV; int16_t navAccNEU[3]; //End of blackbox states +#ifdef USE_AUTO_TRANSITION +typedef struct navMixerATMissionTransition_s { + mixerProfileATRequest_e request; + int32_t heading; + bool active; + bool waypointAcceptedForAdvance; + 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 transitionHoldPos; +} navMixerATMissionTransition_t; + +typedef struct navMixerATMissionCapture_s { + bool active; + vtolMcProtectionSettleState_t settle; +} navMixerATMissionCapture_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_WAIT, + NAV_MISSION_VTOL_TRANSITION_START, + NAV_MISSION_VTOL_TRANSITION_FAIL_ACTION, + NAV_MISSION_VTOL_TRANSITION_REJECT, +} navMissionVtolTransitionDisposition_e; + +static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; +static mixerProfileATRequest_e navMixerATRequestOverride = MIXERAT_REQUEST_NONE; +static bool navVtolFwToMcProtectionLatched; +static navMixerATMissionTransition_t navMixerATMissionTransition; +static navMixerATMissionCapture_t navMixerATMissionCapture; +#else +static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; +#endif +static int8_t waypointPreActionPreparedIndex = -1; + static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode); static void updateDesiredRTHAltitude(void); static void resetAltitudeController(bool useTerrainFollowing); @@ -292,13 +372,28 @@ static void resetJumpCounter(void); static void clearJumpCounters(void); static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); +static bool prepareActiveWaypointForPreAction(const navWaypoint_t *waypoint); +#ifdef USE_AUTO_TRANSITION +static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos); +#endif void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayPos(fpVector3_t * farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance); +bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing); bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); static bool isWaypointMissionValid(void); +#ifdef USE_AUTO_TRANSITION +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); +#endif void missionPlannerSetWaypoint(void); void initializeRTHSanityChecker(void); @@ -351,6 +446,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState); +#ifdef USE_AUTO_TRANSITION +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_MISSION_CAPTURE(navigationFSMState_t previousState); +#endif +#ifdef USE_AUTO_TRANSITION +static bool beginNavigationFwToMcProtectionTransition(void); +#endif #ifdef USE_FW_AUTOLAND static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState); @@ -640,6 +741,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -683,6 +785,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -702,6 +805,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -787,7 +891,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } @@ -811,6 +918,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -858,6 +966,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -1048,8 +1157,18 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT, +#ifdef USE_AUTO_TRANSITION + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, +#endif [NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, //switch to its pending state [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, //switch to its pending state +#ifdef USE_AUTO_TRANSITION + [NAV_FSM_EVENT_MIXERAT_MISSION_ADVANCE] = NAV_STATE_WAYPOINT_NEXT, + [NAV_FSM_EVENT_MIXERAT_MISSION_CAPTURE] = NAV_STATE_MIXERAT_MISSION_CAPTURE, + [NAV_FSM_EVENT_MIXERAT_MISSION_RESUME] = NAV_STATE_WAYPOINT_IN_PROGRESS, +#endif } }, [NAV_STATE_MIXERAT_ABORT] = { @@ -1067,6 +1186,30 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, +#ifdef USE_AUTO_TRANSITION + [NAV_STATE_MIXERAT_MISSION_CAPTURE] = { + .persistentId = NAV_PERSISTENT_ID_MIXERAT_MISSION_CAPTURE, + .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_MISSION_CAPTURE, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, + .mwState = MW_NAV_STATE_WP_ENROUTE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_MISSION_CAPTURE, + [NAV_FSM_EVENT_MIXERAT_MISSION_RESUME] = NAV_STATE_WAYPOINT_IN_PROGRESS, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [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_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, + } + }, +#endif + /** Advanced Fixed Wing Autoland **/ #ifdef USE_FW_AUTOLAND [NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = { @@ -1262,7 +1405,30 @@ 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; +#ifdef USE_AUTO_TRANSITION + const bool mixerATState = (state == NAV_STATE_MIXERAT_INITIALIZE || state == NAV_STATE_MIXERAT_IN_PROGRESS); + + // During mission-authorized MC->FW transition, command heading only. The + // pusher/tilt transition should build speed without the MC position + // controller saturating pitch/roll toward a far-away target. + if (mixerATState && + navMixerATPendingState == NAV_STATE_WAYPOINT_PRE_ACTION && + navMixerATMissionTransition.active && + navMixerATMissionTransition.request == MIXERAT_REQUEST_MISSION_TO_FW) { + stateFlags |= NAV_CTL_YAW | NAV_REQUIRE_MAGHOLD; + } + + // During one-shot retry scan/alignment, command heading only. Avoid XY + // position chase while the VTOL is recovering from a failed transition. + if (mixerATState && + navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE && + isTransitionRetryToFixedWingRequest(navMixerATMissionTransition.request)) { + stateFlags |= NAV_CTL_YAW | NAV_REQUIRE_MAGHOLD; + } +#endif + + return stateFlags; } flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) @@ -1286,6 +1452,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState { UNUSED(previousState); +#ifdef USE_AUTO_TRANSITION + navMixerATPendingState = NAV_STATE_IDLE; + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + navVtolFwToMcProtectionLatched = false; + clearMissionVTOLTransitionState(); +#endif resetAltitudeController(false); resetHeadingController(); resetPositionController(); @@ -1660,6 +1832,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + if (!rthTrackBackSetNewPosition()) { return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; } @@ -1678,7 +1856,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (checkMixerATRequired(MIXERAT_REQUEST_RTH) && (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))){ +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + +#ifdef USE_AUTO_TRANSITION + const bool allowRthMcToFwTransition = !navVtolFwToMcProtectionLatched; +#else + const bool allowRthMcToFwTransition = true; +#endif + + if (allowRthMcToFwTransition && + checkMixerATRequired(MIXERAT_REQUEST_RTH) && + (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))) { return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } @@ -1766,6 +1958,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LAN return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + // Action delay before landing if in FS and option enabled bool pauseLanding = false; navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing; @@ -1781,14 +1979,60 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LAN posControl.landingDelay = 0; } + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); + const bool landingAllowed = navigationRTHAllowsLanding(); + const bool headingReached = ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15); + const bool vtolMcProtectionActive = navigationVtolMcProtectionIsNavActive(); + bool vtolLandingSettleReady = false; + bool vtolLandingSettleConditionsMet = false; + bool vtolLandingSettleChecked = false; + + if (!pauseLanding && landingAllowed && vtolMcProtectionActive) { + if (headingReached) { + vtolLandingSettleReady = navigationVtolMcProtectionLandingSettleReady(tmpHomePos); + vtolLandingSettleChecked = true; + } else { + vtolLandingSettleConditionsMet = navigationVtolMcProtectionLandingSettleConditionsMet(tmpHomePos); + navigationVtolMcProtectionResetLandingSettle(); + } + } + + const bool vtolYawSettleAssistActive = vtolMcProtectionRthLandingYawSettleAssistActive( + vtolMcProtectionActive, + landingAllowed, + vtolLandingSettleConditionsMet, + headingReached, + STATE(FIXED_WING_LEGACY)); + + if (!pauseLanding && vtolYawSettleAssistActive) { + // VTOL MC has already settled over the landing point, but yaw has not + // reached the required heading yet. Hold the current XY/Z target so + // position corrections do not keep competing with yaw authority. + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + return NAV_FSM_EVENT_NONE; + } + // If landing is not temporarily paused (FS only), position ok, OR within valid timeout - continue // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing - if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) { + if (!pauseLanding && (headingReached || STATE(FIXED_WING_LEGACY))) { + + if (landingAllowed && vtolLandingSettleChecked && !vtolLandingSettleReady) { + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + return NAV_FSM_EVENT_NONE; + } + + if (landingAllowed && !vtolLandingSettleChecked && !navigationVtolMcProtectionLandingSettleReady(tmpHomePos)) { + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + return NAV_FSM_EVENT_NONE; + } + resetLandingDetector(); // force reset landing detector just in case updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land + return landingAllowed ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land } else { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_NONE; } @@ -1803,6 +2047,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(n return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LOITER); setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); @@ -1829,9 +2079,25 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF } if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){ + // Fixed-wing VTOL can only pass through the landing area; switch to MC + // before applying the MC-only settle gate below. return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } + const fpVector3_t *landingSettlePos = FLIGHT_MODE(NAV_WP_MODE) ? &posControl.activeWaypoint.pos : rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND); + if (navigationVtolMcProtectionLandingDescentNeedsResettle() || + !navigationVtolMcProtectionLandingSettleReady(landingSettlePos)) { + fpVector3_t landingHoldPos = *landingSettlePos; + landingHoldPos.z = navGetCurrentActualPositionAndVelocity()->pos.z; + if (FLIGHT_MODE(NAV_WP_MODE)) { + setDesiredPosition(&landingHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + } else { + setDesiredPosition(&landingHoldPos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + } + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + return NAV_FSM_EVENT_NONE; + } + #ifdef USE_FW_AUTOLAND if (STATE(AIRPLANE)) { int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8, approachSettingIdx = -1; @@ -1934,6 +2200,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav // Prepare controllers resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled - WP uses global altitude, not AGL + waypointPreActionPreparedIndex = -1; #ifdef USE_FW_AUTOLAND if (previousState != NAV_STATE_FW_LANDING_ABORT) { @@ -1969,6 +2236,618 @@ static navigationFSMEvent_t nextForNonGeoStates(void) } } +#ifdef USE_AUTO_TRANSITION +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); +} + +static uint16_t missionVTOLTransitionAltitudeToleranceCm(void) +{ + return navConfig()->general.waypoint_enforce_altitude > 0 ? + navConfig()->general.waypoint_enforce_altitude : + NAV_MIXERAT_MISSION_TRANSITION_ALTITUDE_FALLBACK_TOLERANCE_CM; +} + +static bool missionVTOLTransitionPointReady(const bool transitionToFixedWing, const uint16_t transitionMinAltitude) +{ + if (!isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) { + return false; + } + + const float currentAltitude = navGetCurrentActualPositionAndVelocity()->pos.z; + + if (transitionMinAltitude > 0 && currentAltitude < transitionMinAltitude) { + return false; + } + + if (transitionToFixedWing && + currentAltitude < (posControl.activeWaypoint.pos.z - missionVTOLTransitionAltitudeToleranceCm())) { + return false; + } + + return true; +} + +#ifdef USE_PITOT +static bool hasUsableTransitionAirspeed(float *airspeedCmS) +{ + if (!sensors(SENSOR_PITOT) || !pitotGetValidForAirspeed() || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE) { + return false; + } + + *airspeedCmS = detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL ? + MAX(getAirspeedEstimate(), 0.0f) : + MAX(pitot.airSpeed, 0.0f); + return true; +} +#else +static bool hasUsableTransitionAirspeed(float *airspeedCmS) +{ + UNUSED(airspeedCmS); + return false; +} +#endif + +static bool beginNavigationFwToMcProtectionTransition(void) +{ + if (navVtolFwToMcProtectionLatched) { + return false; + } + + const uint16_t thresholdCmS = systemConfig()->vtolFwToMcAutoSwitchAirspeed; + float airspeedCmS = 0.0f; + const bool usableAirspeedAvailable = hasUsableTransitionAirspeed(&airspeedCmS); + + if (!mixerTransitionFwToMcProtectionTriggered(ARMING_FLAG(ARMED), STATE(AIRPLANE), thresholdCmS, usableAirspeedAvailable, airspeedCmS)) { + return false; + } + + if (!checkMixerATRequired(MIXERAT_REQUEST_FW_TO_MC_PROTECTION)) { + return false; + } + + navMixerATRequestOverride = MIXERAT_REQUEST_FW_TO_MC_PROTECTION; + navVtolFwToMcProtectionLatched = true; + return true; +} + +static bool isTransitionRetryToFixedWingRequest(const mixerProfileATRequest_e request) +{ + return request == MIXERAT_REQUEST_MISSION_TO_FW || request == MIXERAT_REQUEST_RTH; +} + +static bool hasAirspeedSensorForTransitionRetry(void) +{ + float airspeedCmS = 0.0f; + return hasUsableTransitionAirspeed(&airspeedCmS); +} + +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 || + request == MIXERAT_REQUEST_FW_TO_MC_PROTECTION; +} + +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) && outputProfileDirectSwitchImmediate(nextMixerProfileIndex); +} + +static void clearMissionVTOLTransitionState(void) +{ + navMixerATMissionTransition.active = false; + navMixerATMissionTransition.request = MIXERAT_REQUEST_NONE; + navMixerATMissionTransition.heading = posControl.actualState.yaw; + navMixerATMissionTransition.waypointAcceptedForAdvance = false; + 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.transitionHoldPos = navGetCurrentActualPositionAndVelocity()->pos; + navMixerATMissionCapture.active = false; + navMixerATMissionCapture.settle.stableSinceMs = 0; + navMixerATMissionCapture.settle.elapsedMs = 0; +} + +static void startMissionVTOLTransitionCapture(void) +{ + navMixerATMissionCapture.active = true; + navMixerATMissionCapture.settle.stableSinceMs = 0; + navMixerATMissionCapture.settle.elapsedMs = 0; +} + +static uint16_t missionVTOLTransitionHorizontalSettleLimitCmS(void) +{ +#ifdef USE_MR_BRAKING_MODE + return vtolMcProtectionHorizontalSettleSpeedCmS(navConfig()->mc.braking_disengage_speed); +#else + return vtolMcProtectionHorizontalSettleSpeedCmS(0); +#endif +} + +static bool missionVTOLTransitionCaptureIsReady(void) +{ + const bool horizontalVelocityUsable = posControl.flags.estVelStatus == EST_TRUSTED; + const bool verticalVelocityUsable = posControl.flags.estAltStatus >= EST_USABLE; + const uint16_t maxAbsAttitudeDeciDeg = MAX(ABS(attitude.values.roll), ABS(attitude.values.pitch)); + const bool conditionsMet = vtolMcProtectionSettleConditionsMetWithFallback( + horizontalVelocityUsable, + verticalVelocityUsable, + posControl.actualState.velXY, + navGetCurrentActualPositionAndVelocity()->vel.z, + maxAbsAttitudeDeciDeg, + missionVTOLTransitionHorizontalSettleLimitCmS(), + vtolMcProtectionVerticalSettleSpeedCmS(navConfig()->general.land_minalt_vspd), + vtolMcProtectionSettleAttitudeLimitDeciDeg(navConfig()->mc.max_bank_angle)); + + return vtolMcProtectionUpdateSettleState( + &navMixerATMissionCapture.settle, + conditionsMet, + vtolMcProtectionSettleTimeMs(horizontalVelocityUsable), + millis()); +} + +static void rebaseMissionWaypointAfterTransition(void) +{ + const fpVector3_t actualPosition = navGetCurrentActualPositionAndVelocity()->pos; + + // The transition was authorized using the old platform's bearing. Restart + // the waypoint geometry from the actual post-transition position instead. + posControl.activeWaypoint.bearing = calculateBearingToDestination(&posControl.activeWaypoint.pos); + posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; + posControl.wpAltitudeReached = false; + posControl.wpAltitudeEnforceActive = false; + posControl.wpAltitudeEnforceFromStart = false; + posControl.wpAltitudeEnforceHoldPos = actualPosition; + posControl.wpReachedTime = 0; +} + +static bool navMixerATStateActive(void) +{ + return posControl.navState == NAV_STATE_MIXERAT_INITIALIZE || + posControl.navState == NAV_STATE_MIXERAT_IN_PROGRESS; +} + +static bool navMissionVTOLTransitionInProgress(void) +{ + return navMixerATMissionTransition.active && navMixerATStateActive(); +} + +static bool navMixerATRequestIsNavigationOwned(const mixerProfileATRequest_e request) +{ + return request == MIXERAT_REQUEST_RTH || + request == MIXERAT_REQUEST_LAND || + request == MIXERAT_REQUEST_MISSION_TO_FW || + request == MIXERAT_REQUEST_MISSION_TO_MC || + request == MIXERAT_REQUEST_FW_TO_MC_PROTECTION; +} + +static mixerProfileATRequest_e navMixerATNavigationOwnedRequest(void) +{ + if (navMixerATRequestOverride != MIXERAT_REQUEST_NONE) { + return navMixerATRequestOverride; + } + + switch (navMixerATPendingState) { + case NAV_STATE_RTH_HEAD_HOME: + return MIXERAT_REQUEST_RTH; + + case NAV_STATE_RTH_LANDING: + return MIXERAT_REQUEST_LAND; + + case NAV_STATE_WAYPOINT_PRE_ACTION: + return navMixerATMissionTransition.active ? navMixerATMissionTransition.request : MIXERAT_REQUEST_NONE; + + default: + return MIXERAT_REQUEST_NONE; + } +} + +static navigationFSMEvent_t navSanitizeTransitionFailEventForCurrentState(navigationFSMEvent_t event) +{ + if (event == NAV_FSM_EVENT_NONE || + navFSM[posControl.navState].onEvent[event] != NAV_STATE_UNDEFINED) { + return event; + } + + if (event == NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME && + navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_SWITCH_TO_RTH] != NAV_STATE_UNDEFINED) { + return NAV_FSM_EVENT_SWITCH_TO_RTH; + } + + return NAV_FSM_EVENT_SWITCH_TO_IDLE; +} + +static navigationFSMEvent_t abortOrphanedNavigationMixerATTransition(void) +{ + if (!mixerATIsActive() || navMixerATStateActive() || mixerProfileAT.request == MIXERAT_REQUEST_NONE) { + return NAV_FSM_EVENT_NONE; + } + + const mixerProfileATRequest_e activeRequest = mixerProfileAT.request; + if (!navMixerATRequestIsNavigationOwned(activeRequest)) { + return NAV_FSM_EVENT_NONE; + } + + const mixerProfileATRequest_e request = navMixerATNavigationOwnedRequest(); + if (request == MIXERAT_REQUEST_NONE || request != activeRequest) { + return NAV_FSM_EVENT_NONE; + } + + const navigationFSMEvent_t failEvent = navSanitizeTransitionFailEventForCurrentState(getTransitionFailEvent(request)); + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + clearMissionVTOLTransitionState(); + return failEvent; +} + +static void beginMissionTransitionRetryScan(const mixerProfileATRequest_e request) +{ + navMixerATMissionTransition.request = request; + // Mission retries need to keep the waypoint pre-action context alive so + // the second attempt can restart MixerAT after scan/align completes. + navMixerATMissionTransition.active = request == MIXERAT_REQUEST_MISSION_TO_FW || + request == MIXERAT_REQUEST_MISSION_TO_MC; + 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; +} + +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 (hasUsableTransitionAirspeed(&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; + + // If low-speed protection already selected MC as the safer fallback, do + // not let later mission USER bits immediately send the aircraft back to FW. + if (transitionToFixedWing && navVtolFwToMcProtectionLatched) { + return NAV_MISSION_VTOL_TRANSITION_CONTINUE; + } + + // Idempotent mission command: continue immediately if already in requested platform state. + if ((transitionToFixedWing && STATE(AIRPLANE)) || (!transitionToFixedWing && STATE(MULTIROTOR))) { + return NAV_MISSION_VTOL_TRANSITION_CONTINUE; + } + + const uint16_t transitionMinAltitude = navConfig()->general.vtol_mission_transition_min_altitude; + + const navMissionVtolTransitionPrecondition_e precondition = navMissionVtolTransitionPreconditionDisposition( + ARMING_FLAG(ARMED), + FLIGHT_MODE(FAILSAFE_MODE), + areSensorsCalibrating(), + posControl.flags.estPosStatus >= EST_USABLE, + posControl.flags.estHeadingStatus >= EST_USABLE, + isModeActivationConditionPresent(BOXMIXERPROFILE), + checkMixerProfileHotSwitchAvalibility(), + mixerATIsActive()); + + if (precondition == NAV_MISSION_VTOL_PRECONDITION_WAIT) { + return NAV_MISSION_VTOL_TRANSITION_WAIT; + } + + if (precondition == NAV_MISSION_VTOL_PRECONDITION_REJECT) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + if (!missionVTOLTransitionPointReady(transitionToFixedWing, transitionMinAltitude)) { + return NAV_MISSION_VTOL_TRANSITION_WAIT; + } + + const flyingPlatformType_e targetPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; + const bool targetPlatformMatchesRequest = transitionToFixedWing ? + targetPlatformType == PLATFORM_AIRPLANE : + isMissionTransitionToMultirotorType(targetPlatformType); + const navMissionVtolTransitionStartValidation_e startValidation = navMissionVtolTransitionStartValidation( + targetPlatformMatchesRequest, + checkMixerATRequired(requestedAction)); + + if (startValidation == NAV_MISSION_VTOL_START_VALIDATION_REJECT) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + if (startValidation == NAV_MISSION_VTOL_START_VALIDATION_FAIL_ACTION) { + navMixerATMissionTransition.request = requestedAction; + return NAV_MISSION_VTOL_TRANSITION_FAIL_ACTION; + } + + 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; + navMixerATMissionTransition.waypointAcceptedForAdvance = navMissionTransitionWaypointAcceptedForAdvance( + true, + waypoint->action == NAV_WP_ACTION_WAYPOINT, + navConfig()->general.waypoint_enforce_altitude, + navGetCurrentActualPositionAndVelocity()->pos.z, + posControl.activeWaypoint.pos.z); + navMixerATMissionTransition.transitionHoldPos = navGetCurrentActualPositionAndVelocity()->pos; + return NAV_MISSION_VTOL_TRANSITION_START; +} + +static void updateMissionTransitionGuidance(void) +{ + if (navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE && + isTransitionRetryToFixedWingRequest(navMixerATMissionTransition.request) && + STATE(MULTIROTOR)) { + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, + navMixerATMissionTransition.retryTargetHeading, + NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return; + } + + if (navMixerATMissionTransition.active && + navMixerATMissionTransition.request == MIXERAT_REQUEST_MISSION_TO_FW && + STATE(MULTIROTOR)) { + setDesiredPosition(&navMixerATMissionTransition.transitionHoldPos, + navMixerATMissionTransition.heading, + NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return; + } + + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); +} + +navVtolTransitionOsdState_e navigationVtolTransitionOsdState(void) +{ + if (posControl.navState != NAV_STATE_MIXERAT_INITIALIZE && + posControl.navState != NAV_STATE_MIXERAT_IN_PROGRESS) { + return NAV_VTOL_TRANSITION_OSD_NONE; + } + + switch (navMixerATMissionTransition.retryStage) { + case NAV_MIXERAT_RETRY_STAGE_SCAN: + return NAV_VTOL_TRANSITION_OSD_RETRY_SCAN; + + case NAV_MIXERAT_RETRY_STAGE_ALIGN: + return NAV_VTOL_TRANSITION_OSD_RETRY_ALIGN; + + case NAV_MIXERAT_RETRY_STAGE_IDLE: + default: + return NAV_VTOL_TRANSITION_OSD_NONE; + } +} +#endif + +static bool prepareActiveWaypointForPreAction(const navWaypoint_t *waypoint) +{ + fpVector3_t localPos; + mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3)); + + const bool waypointChanged = + waypointPreActionPreparedIndex != posControl.activeWaypointIndex || + posControl.activeWaypoint.pos.x != localPos.x || + posControl.activeWaypoint.pos.y != localPos.y || + posControl.activeWaypoint.pos.z != localPos.z; + + if (!waypointChanged) { + return false; + } + + calculateAndSetActiveWaypoint(waypoint); + posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; + posControl.wpAltitudeReached = false; + posControl.wpAltitudeEnforceActive = false; + posControl.wpAltitudeEnforceFromStart = false; + posControl.wpAltitudeEnforceHoldPos = navGetCurrentActualPositionAndVelocity()->pos; + waypointPreActionPreparedIndex = posControl.activeWaypointIndex; + + return true; +} + +static bool isGeoWaypointAction(const navWaypointActions_e action) +{ + return action == NAV_WP_ACTION_WAYPOINT || + action == NAV_WP_ACTION_HOLD_TIME || + action == NAV_WP_ACTION_LAND; +} + +static bool isActiveWaypointFirstGeoWaypoint(void) +{ + for (int8_t waypointIndex = posControl.startWpIndex; waypointIndex <= posControl.activeWaypointIndex; waypointIndex++) { + if (isGeoWaypointAction((navWaypointActions_e)posControl.waypointList[waypointIndex].action)) { + return waypointIndex == posControl.activeWaypointIndex; + } + } + + return false; +} + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState) { /* A helper function to do waypoint-specific action */ @@ -1977,12 +2856,36 @@ 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]); - posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); - posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; - posControl.wpAltitudeReached = false; + case NAV_WP_ACTION_LAND: { + const navWaypoint_t * const activeWaypoint = &posControl.waypointList[posControl.activeWaypointIndex]; + + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + + prepareActiveWaypointForPreAction(activeWaypoint); + +#ifdef USE_AUTO_TRANSITION + clearMissionVTOLTransitionState(); + const navMissionVtolTransitionDisposition_e transitionAction = prepareMissionVTOLTransition(activeWaypoint); + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_WAIT) { + return NAV_FSM_EVENT_NONE; + } + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_START) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_FAIL_ACTION) { + const navigationFSMEvent_t failEvent = getTransitionFailEvent(navMixerATMissionTransition.request); + clearMissionVTOLTransitionState(); + return failEvent; + } + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_REJECT) { + return NAV_FSM_EVENT_ERROR; + } +#endif + 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) @@ -2033,11 +2936,84 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na // If no position sensor available - land immediately if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) { +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_HOLD_TIME: case NAV_WP_ACTION_WAYPOINT: - case NAV_WP_ACTION_LAND: - if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) { + case NAV_WP_ACTION_LAND: { + const bool waypointReached = isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing); + const bool forceAltitudeFromWaypointStart = navMissionShouldForceFirstGeoWaypointAltitudeFromStart( + STATE(MULTIROTOR), + isActiveWaypointFirstGeoWaypoint()); + const bool wasAltitudeEnforceActive = posControl.wpAltitudeEnforceActive; + posControl.wpAltitudeEnforceActive = navMissionUpdateMultirotorWaypointAltitudeEnforceActive( + STATE(MULTIROTOR), + posControl.wpAltitudeEnforceActive, + forceAltitudeFromWaypointStart, + navConfig()->general.waypoint_enforce_altitude, + posControl.wpDistance, + posControl.wpInitialDistance, + navConfig()->general.waypoint_radius); + + if (waypointReached || posControl.wpAltitudeEnforceActive) { + const float waypointAltitudeError = fabsf(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z); + if (!wasAltitudeEnforceActive && + posControl.wpAltitudeEnforceActive && + forceAltitudeFromWaypointStart) { + posControl.wpAltitudeEnforceFromStart = true; + } + + if (posControl.wpAltitudeEnforceActive && + navConfig()->general.waypoint_enforce_altitude > 0 && + waypointAltitudeError < navConfig()->general.waypoint_enforce_altitude) { + posControl.wpAltitudeReached = true; + posControl.wpAltitudeEnforceFromStart = false; + } + + if (navMissionShouldHoldMultirotorWaypointForAltitude( + STATE(MULTIROTOR), + posControl.wpAltitudeEnforceActive, + navConfig()->general.waypoint_enforce_altitude, + posControl.wpDistance, + posControl.wpInitialDistance, + navConfig()->general.waypoint_radius, + waypointAltitudeError)) { + const bool holdFirstWaypointXy = navMissionShouldHoldFirstMultirotorWaypointXyForAltitude( + STATE(MULTIROTOR), + posControl.wpAltitudeEnforceFromStart, + navConfig()->general.waypoint_enforce_altitude, + waypointAltitudeError); + fpVector3_t altitudeHoldPos = holdFirstWaypointXy ? + posControl.wpAltitudeEnforceHoldPos : + posControl.activeWaypoint.pos; + altitudeHoldPos.z = posControl.activeWaypoint.pos.z; + const navSetWaypointFlags_t altitudeHoldUpdateFlags = holdFirstWaypointXy ? + (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z) : + (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + setDesiredPosition(&altitudeHoldPos, 0, altitudeHoldUpdateFlags); + + if (STATE(MULTIROTOR)) { + switch (wpHeadingControl.mode) { + case NAV_WP_HEAD_MODE_NONE: + break; + case NAV_WP_HEAD_MODE_FIXED: + setDesiredPosition(NULL, wpHeadingControl.heading, NAV_POS_UPDATE_HEADING); + break; + case NAV_WP_HEAD_MODE_POI: + setDesiredPosition(&wpHeadingControl.poi_pos, 0, NAV_POS_UPDATE_BEARING); + break; + } + } + return NAV_FSM_EVENT_NONE; + } + } + + if (waypointReached) { return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED } else { @@ -2045,9 +3021,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.y = posControl.activeWaypoint.pos.y; /* Use linear climb/descent between WPs arriving at WP altitude when within 10% of total distance to WP */ - tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, 0.1f * posControl.wpInitialDistance, posControl.wpInitialDistance), - posControl.wpInitialDistance, 0.1f * posControl.wpInitialDistance, - posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); + const float interpolatedAltitude = scaleRangef(constrainf(posControl.wpDistance, 0.1f * posControl.wpInitialDistance, posControl.wpInitialDistance), + posControl.wpInitialDistance, 0.1f * posControl.wpInitialDistance, + posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); + tmpWaypoint.z = navMissionMultirotorWaypointAltitudeTarget( + STATE(MULTIROTOR), + posControl.wpAltitudeEnforceActive, + posControl.activeWaypoint.pos.z, + interpolatedAltitude); setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); @@ -2066,6 +3047,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } break; + } case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_SET_HEAD: @@ -2138,6 +3120,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { // Adjust altitude to waypoint setting setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z); @@ -2309,7 +3297,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); @@ -2319,7 +3306,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navi resetAltitudeController(false); setupAltitudeController(); } + +#ifdef USE_AUTO_TRANSITION + if (previousState != NAV_STATE_WAYPOINT_PRE_ACTION) { + clearMissionVTOLTransitionState(); + } + + updateMissionTransitionGuidance(); +#else setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); +#endif navMixerATPendingState = previousState; return NAV_FSM_EVENT_SUCCESS; } @@ -2327,6 +3323,173 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState) { UNUSED(previousState); + +#ifdef USE_AUTO_TRANSITION + mixerProfileATRequest_e required_action; + if (navMixerATRequestOverride != MIXERAT_REQUEST_NONE) { + required_action = navMixerATRequestOverride; + } else { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + required_action = MIXERAT_REQUEST_RTH; + break; + case NAV_STATE_RTH_LANDING: + required_action = MIXERAT_REQUEST_LAND; + break; + case NAV_STATE_WAYPOINT_PRE_ACTION: + required_action = navMixerATMissionTransition.active ? navMixerATMissionTransition.request : MIXERAT_REQUEST_NONE; + break; + default: + required_action = MIXERAT_REQUEST_NONE; + break; + } + } + + if (!ARMING_FLAG(ARMED) || + (FLIGHT_MODE(FAILSAFE_MODE) && mixerTransitionShouldAbortForFailsafe(required_action, false, false))) { + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + clearMissionVTOLTransitionState(); + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + 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); + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + 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 mixerATAborted = transitionAborted; + 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; + } + + const bool forcedProfileSwitchCompleted = transitionAborted && + tryForceSwitchAfterFwToMcFailure(required_action); + if (forcedProfileSwitchCompleted) { + transitionAborted = false; + } + + const bool transitionReachedTargetProfile = navMissionTransitionReachedTargetProfile( + mixerATAborted, + forcedProfileSwitchCompleted); + + navigationFSMEvent_t nextEvent = NAV_FSM_EVENT_SWITCH_TO_IDLE; + bool startMissionCapture = false; + bool rebaseMissionWaypoint = false; + if (transitionAborted) { + nextEvent = getTransitionFailEvent(required_action); + } else { + const bool advanceMissionWaypoint = missionTransitionWasActive && + navMissionShouldAdvanceWaypointAfterTransition( + navMixerATMissionTransition.waypointAcceptedForAdvance, + transitionReachedTargetProfile, + navConfig()->general.waypoint_enforce_altitude, + navGetCurrentActualPositionAndVelocity()->pos.z, + posControl.activeWaypoint.pos.z); + if (required_action == MIXERAT_REQUEST_FW_TO_MC_PROTECTION) { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_TRACKBACK: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH; + break; + case NAV_STATE_RTH_HEAD_HOME: + case NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LOITER_ABOVE_HOME: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH; + break; + case NAV_STATE_WAYPOINT_IN_PROGRESS: + case NAV_STATE_WAYPOINT_HOLD_TIME: + nextEvent = NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; + break; + default: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + break; + } + } else { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LANDING: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + break; + case NAV_STATE_WAYPOINT_PRE_ACTION: + if (missionTransitionWasActive) { + startMissionCapture = navMissionTransitionShouldCaptureBeforeWaypointResume( + missionTransitionWasActive, + !transitionAborted, + required_action == MIXERAT_REQUEST_MISSION_TO_MC, + STATE(MULTIROTOR), + advanceMissionWaypoint); + rebaseMissionWaypoint = navMissionTransitionShouldRebaseWaypointBeforeResume( + missionTransitionWasActive, + !transitionAborted, + advanceMissionWaypoint, + startMissionCapture); + nextEvent = advanceMissionWaypoint ? + NAV_FSM_EVENT_MIXERAT_MISSION_ADVANCE : + (startMissionCapture ? NAV_FSM_EVENT_MIXERAT_MISSION_CAPTURE : 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); + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + clearMissionVTOLTransitionState(); + if (startMissionCapture) { + startMissionVTOLTransitionCapture(); + } else if (rebaseMissionWaypoint) { + rebaseMissionWaypointAfterTransition(); + } + if (nextEvent != NAV_FSM_EVENT_SWITCH_TO_IDLE) { + setupAltitudeController(); + } + return nextEvent; + } + + updateMissionTransitionGuidance(); + + return NAV_FSM_EVENT_NONE; +#else mixerProfileATRequest_e required_action; switch (navMixerATPendingState) { @@ -2364,15 +3527,52 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(nav setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); return NAV_FSM_EVENT_NONE; +#endif } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState) { UNUSED(previousState); mixerATUpdateState(MIXERAT_REQUEST_ABORT); +#ifdef USE_AUTO_TRANSITION + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + clearMissionVTOLTransitionState(); +#endif return NAV_FSM_EVENT_SUCCESS; } +#ifdef USE_AUTO_TRANSITION +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_MISSION_CAPTURE(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + // Match normal waypoint navigation: capture still depends on position and + // heading estimates, so do not remain in it after a sensor timeout. + if (checkForPositionSensorTimeout() || posControl.flags.estHeadingStatus == EST_NONE) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + + if (!navMixerATMissionCapture.active || !STATE(MULTIROTOR)) { + return NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; + } + + // Keep the target at the real vehicle position until its fixed-wing + // momentum has decayed. This damps velocity without rubber-banding to the + // transition waypoint. + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, + posControl.actualState.yaw, + NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + + if (!missionVTOLTransitionCaptureIsReady()) { + return NAV_FSM_EVENT_NONE; + } + + navMixerATMissionCapture.active = false; + rebaseMissionWaypointAfterTransition(); + return NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; +} +#endif + #ifdef USE_FW_AUTOLAND static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState) { @@ -2704,6 +3904,16 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) lastStateProcessTime = currentMillis; } +#ifdef USE_AUTO_TRANSITION + if (navMixerATMissionCapture.active && posControl.navState != NAV_STATE_MIXERAT_MISSION_CAPTURE) { + // A mode/failsafe change interrupted capture. Never leak it into a + // later mission or navigation session. + navMixerATMissionCapture.active = false; + navMixerATMissionCapture.settle.stableSinceMs = 0; + navMixerATMissionCapture.settle.elapsedMs = 0; + } +#endif + /* Update public system state information */ NAV_Status.mode = MW_GPS_MODE_NONE; @@ -4556,6 +5766,19 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) const bool canActivatePosHold = canActivatePosHoldMode(); const bool canActivateNavigation = canActivateNavigationModes(); const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH; +#ifdef USE_AUTO_TRANSITION + const navigationFSMEvent_t orphanedTransitionEvent = abortOrphanedNavigationMixerATTransition(); + if (orphanedTransitionEvent != NAV_FSM_EVENT_NONE) { + return orphanedTransitionEvent; + } + + const bool missionVtolTransitionInProgress = navMissionVTOLTransitionInProgress(); + const bool waypointRestartGuardSuppressed = navMissionVtolTransitionSuppressesWaypointRestartGuard(missionVtolTransitionInProgress); + const bool waypointFallbackToRthSuppressed = navMissionVtolTransitionSuppressesWaypointFallbackToRth(missionVtolTransitionInProgress); +#else + const bool waypointRestartGuardSuppressed = false; + const bool waypointFallbackToRthSuppressed = false; +#endif #ifdef USE_SAFE_HOME checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated); #endif @@ -4651,7 +5874,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) if (IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive) { canActivateWaypoint = false; } else { - if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) { + if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE) && !waypointRestartGuardSuppressed) { canActivateWaypoint = false; if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { @@ -4660,7 +5883,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } } - wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint; + wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !waypointFallbackToRthSuppressed; } /* Pilot-triggered RTH, also fall-back for WP if no mission is loaded. @@ -4679,6 +5902,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_IDLE; } +#ifdef USE_AUTO_TRANSITION + if (navMissionVtolTransitionHoldsWaypointSelector(missionVtolTransitionInProgress, IS_RC_MODE_ACTIVE(BOXNAVWP))) { + waypointWasActivated = true; + return NAV_FSM_EVENT_NONE; + } +#endif + // Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded. // Also check multimission mission change status before activating WP mode. #ifdef USE_MULTI_MISSION @@ -5109,6 +6339,12 @@ void navigationInit(void) { /* Initial state */ posControl.navState = NAV_STATE_IDLE; +#ifdef USE_AUTO_TRANSITION + navMixerATPendingState = NAV_STATE_IDLE; + navMixerATRequestOverride = MIXERAT_REQUEST_NONE; + navVtolFwToMcProtectionLatched = false; + clearMissionVTOLTransitionState(); +#endif posControl.flags.horizontalPositionDataNew = false; posControl.flags.verticalPositionDataNew = false; @@ -5533,6 +6769,9 @@ static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; posControl.wpAltitudeReached = false; + posControl.wpAltitudeEnforceActive = false; + posControl.wpAltitudeEnforceFromStart = false; + posControl.wpAltitudeEnforceHoldPos = navGetCurrentActualPositionAndVelocity()->pos; } void resetFwAutolandApproach(int8_t idx) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b6cf4692b66..3abeee04e22 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -328,6 +328,31 @@ typedef enum { WP_MISSION_SWITCH, } navMissionRestart_e; +#ifdef USE_AUTO_TRANSITION +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; +#endif + typedef enum { RTH_TRACKBACK_OFF, RTH_TRACKBACK_ON, @@ -413,6 +438,13 @@ typedef struct navConfig_s { uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance +#ifdef USE_AUTO_TRANSITION + uint8_t vtol_mission_transition_user_action; // User action slot that requests mission VTOL transition + uint16_t vtol_mission_transition_min_altitude; // Minimum altitude [cm] to start mission VTOL transition (0 = disabled) + bool vtol_transition_retry_on_airspeed_timeout; // Enables one-shot yaw-scan retry for failed airspeed-gated MC->FW auto-transition + uint8_t vtol_transition_fail_action_mc_to_fw; // Action after final MC->FW transition failure + uint8_t vtol_transition_fail_action_fw_to_mc; // Action after final FW->MC transition failure +#endif #ifdef USE_MULTI_MISSION uint8_t waypoint_multi_mission_index; // Index of mission to be loaded in multi mission entry #endif @@ -458,7 +490,6 @@ typedef struct navConfig_s { uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase #endif - uint8_t posDecelerationTime; // Brake time parameter uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC) bool slowDownForTurning; // Slow down during WP missions when changing heading on next waypoint @@ -653,6 +684,14 @@ typedef enum { MW_NAV_FLAG_ADJUSTING_ALTITUDE = 1 << 1, } navSystemStatus_Flags_e; +#ifdef USE_AUTO_TRANSITION +typedef enum { + NAV_VTOL_TRANSITION_OSD_NONE = 0, + NAV_VTOL_TRANSITION_OSD_RETRY_SCAN, + NAV_VTOL_TRANSITION_OSD_RETRY_ALIGN, +} navVtolTransitionOsdState_e; +#endif + typedef struct { navSystemStatus_Mode_e mode; navSystemStatus_State_e state; @@ -793,6 +832,9 @@ bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int3 */ bool navigationRTHAllowsLanding(void); bool isWaypointMissionRTHActive(void); +#ifdef USE_AUTO_TRANSITION +navVtolTransitionOsdState_e navigationVtolTransitionOsdState(void); +#endif bool rthClimbStageActiveAndComplete(void); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 9b6646952c8..1a3b426427f 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -51,6 +51,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" #include "navigation/sqrt_controller.h" +#include "navigation/navigation_vtol_mc_protection.h" #include "sensors/battery.h" @@ -64,6 +65,80 @@ static pt1Filter_t altholdThrottleFilterState; static bool prepareForTakeoffOnReset = false; static sqrt_controller_t alt_hold_sqrt_controller; +// A winged VTOL can satisfy passive landed thresholds while still airborne. +// Confirm touchdown by lightly reducing lift throttle and watching for descent. +typedef struct mcLandingProbeState_s { + bool active; + timeMs_t startedAtMs; + int16_t startThrottle; + bool startAglTrusted; + float startAglCm; + float startVerticalSpeedCmS; +} mcLandingProbeState_t; + +static mcLandingProbeState_t mcLandingProbe; + +static void setMulticopterCurrentPositionAsHoldTarget(navSetWaypointFlags_t useMask) +{ + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, useMask); +} + +static void resetMulticopterLandingProbe(void) +{ + mcLandingProbe = (mcLandingProbeState_t){ 0 }; +} + +static void startMulticopterLandingProbe(timeMs_t currentTimeMs) +{ + mcLandingProbe.active = true; + mcLandingProbe.startedAtMs = currentTimeMs; + mcLandingProbe.startThrottle = rcCommandAdjustedThrottle; + mcLandingProbe.startAglTrusted = posControl.flags.estAglStatus == EST_TRUSTED; + mcLandingProbe.startAglCm = posControl.actualState.agl.pos.z; + mcLandingProbe.startVerticalSpeedCmS = navGetCurrentActualPositionAndVelocity()->vel.z; +} + +static bool updateMulticopterLandingProbe(timeMs_t currentTimeMs, bool *probeAborted) +{ + *probeAborted = false; + + if (!mcLandingProbe.active) { + startMulticopterLandingProbe(currentTimeMs); + return false; + } + + if (vtolMcProtectionLandingProbeAirborneResponse( + mcLandingProbe.startAglTrusted && posControl.flags.estAglStatus == EST_TRUSTED, + mcLandingProbe.startAglCm, + posControl.actualState.agl.pos.z, + posControl.flags.estAltStatus >= EST_USABLE, + mcLandingProbe.startVerticalSpeedCmS, + navGetCurrentActualPositionAndVelocity()->vel.z, + acc.accADCf[Z])) { + *probeAborted = true; + resetMulticopterLandingProbe(); + return false; + } + + return currentTimeMs - mcLandingProbe.startedAtMs >= VTOL_MC_LANDING_PROBE_CONFIRM_TIME_MS; +} + +static int16_t applyMulticopterLandingProbeThrottle(const int16_t requestedThrottle, const int16_t idleThrottle, const int16_t hoverThrottle) +{ + if (!mcLandingProbe.active) { + return requestedThrottle; + } + + const int16_t probeThrottle = vtolMcProtectionLandingProbeThrottle( + mcLandingProbe.startThrottle, + idleThrottle, + hoverThrottle, + mcLandingProbe.startedAtMs, + millis()); + + return requestedThrottle < probeThrottle ? requestedThrottle : probeThrottle; +} + float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros) { return sqrtControllerApply( @@ -77,6 +152,8 @@ float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros) // Position to velocity controller for Z axis static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) { + navigationVtolMcProtectionApplySoftAltitudeCapture(navGetCurrentStateFlags()); + float targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); posControl.pids.pos[Z].output_constrained = targetVel; // only used for Blackbox and OSD info @@ -108,15 +185,24 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) { // Calculate min and max throttle boundaries (to compensate for integral windup) - const int16_t thrCorrectionMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle; - const int16_t thrCorrectionMax = getMaxThrottle() - currentBatteryProfile->nav.mc.hover_throttle; - - float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, 0); + const int16_t idleThrottle = getThrottleIdleValue(); + const int16_t hoverThrottle = currentBatteryProfile->nav.mc.hover_throttle; + const int16_t maxThrottle = getMaxThrottle(); + const vtolMcProtectionThrottleBounds_t vtolMcThrottleBounds = navigationVtolMcProtectionGetThrottleBounds(idleThrottle, hoverThrottle, maxThrottle); + const int16_t thrCorrectionMin = vtolMcThrottleBounds.min - hoverThrottle; + const int16_t thrCorrectionMax = vtolMcThrottleBounds.max - hoverThrottle; + const pidControllerFlags_e pidFlags = (navigationVtolMcProtectionShouldFreezeAltitudeIntegrator() || mcLandingProbe.active) ? PID_FREEZE_INTEGRATOR : 0; + + float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, pidFlags); int16_t rcThrottleCorrection = pt1FilterApply3(&altholdThrottleFilterState, velocity_controller, US2S(deltaMicros)); rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax); - posControl.rcAdjustment[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, false); + int16_t protectedThrottle = hoverThrottle + rcThrottleCorrection; + protectedThrottle = navigationVtolMcProtectionApplyBailoutThrottle(protectedThrottle, &vtolMcThrottleBounds, hoverThrottle); + protectedThrottle = applyMulticopterLandingProbeThrottle(protectedThrottle, idleThrottle, hoverThrottle); + posControl.rcAdjustment[THROTTLE] = setDesiredThrottle(protectedThrottle, false); + navigationVtolMcProtectionPublishThrottleDebug(&vtolMcThrottleBounds, posControl.rcAdjustment[THROTTLE]); } bool adjustMulticopterAltitudeFromRCInput(void) @@ -174,6 +260,8 @@ bool adjustMulticopterAltitudeFromRCInput(void) void setupMulticopterAltitudeController(void) { + resetMulticopterLandingProbe(); + const bool throttleIsLow = throttleStickIsLow(); const uint8_t throttleType = navConfig()->mc.althold_throttle_type; @@ -200,6 +288,8 @@ void setupMulticopterAltitudeController(void) void resetMulticopterAltitudeController(void) { + resetMulticopterLandingProbe(); + const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity(); float nav_speed_up = 0.0f; float nav_speed_down = 0.0f; @@ -307,17 +397,41 @@ void resetMulticopterBrakingMode(void) DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED); } +bool navigationMulticopterBrakingActive(void) +{ +#ifdef USE_MR_BRAKING_MODE + return STATE(NAV_CRUISE_BRAKING); +#else + return false; +#endif +} + +bool navigationMulticopterBrakingBoostActive(void) +{ +#ifdef USE_MR_BRAKING_MODE + return STATE(NAV_CRUISE_BRAKING_BOOST); +#else + return false; +#endif +} + static void processMulticopterBrakingMode(const bool isAdjusting) { #ifdef USE_MR_BRAKING_MODE static uint32_t brakingModeDisengageAt = 0; static uint32_t brakingBoostModeDisengageAt = 0; + const bool brakingSuppressed = vtolMcProtectionSuppressesMulticopterBrakingMode(navigationVtolMcProtectionIsNavActive()); if (!(NAV_Status.state == MW_NAV_STATE_NONE || NAV_Status.state == MW_NAV_STATE_HOLD_INFINIT)) { resetMulticopterBrakingMode(); return; } + if (brakingSuppressed) { + resetMulticopterBrakingMode(); + return; + } + const bool brakingEntryAllowed = IS_RC_MODE_ACTIVE(BOXBRAKING) && !STATE(NAV_CRUISE_BRAKING_LOCKED) && @@ -337,7 +451,7 @@ static void processMulticopterBrakingMode(const bool isAdjusting) * Set currnt position and target position * Enabling NAV_CRUISE_BRAKING locks other routines from setting position! */ - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); + setMulticopterCurrentPositionAsHoldTarget(NAV_POS_UPDATE_XY); ENABLE_STATE(NAV_CRUISE_BRAKING_LOCKED); ENABLE_STATE(NAV_CRUISE_BRAKING); @@ -385,7 +499,7 @@ static void processMulticopterBrakingMode(const bool isAdjusting) * When braking is done, store current position as desired one * We do not want to go back to the place where braking has started */ - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); + setMulticopterCurrentPositionAsHoldTarget(NAV_POS_UPDATE_XY); } #else UNUSED(isAdjusting); @@ -595,7 +709,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA float maxAccelChange = US2S(deltaMicros) * MC_POS_CONTROL_JERK_LIMIT_CMSSS; //When braking, raise jerk limit even if we are not boosting acceleration #ifdef USE_MR_BRAKING_MODE - if (STATE(NAV_CRUISE_BRAKING)) { + if (navigationMulticopterBrakingActive() && + !vtolMcProtectionSuppressesMulticopterBrakingMode(navigationVtolMcProtectionIsNavActive())) { maxAccelChange = maxAccelChange * 2; } #endif @@ -661,7 +776,9 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA #ifdef USE_MR_BRAKING_MODE //Boost required accelerations - if (STATE(NAV_CRUISE_BRAKING_BOOST) && multicopterPosXyCoefficients.breakingBoostFactor > 0.0f) { + if (navigationMulticopterBrakingBoostActive() && + !vtolMcProtectionSuppressesMulticopterBrakingMode(navigationVtolMcProtectionIsNavActive()) && + multicopterPosXyCoefficients.breakingBoostFactor > 0.0f) { //Scale boost factor according to speed const float boostFactor = constrainf( @@ -717,6 +834,13 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI && posControl.flags.isAdjustingPosition; + // Pilot position input always takes priority over the VTOL braking + // capture. This also prevents a stale capture from overwriting the stop + // target established when the stick is released. + if (posControl.flags.isAdjustingPosition) { + navigationVtolMcProtectionResetCapture(); + } + if (posControl.flags.horizontalPositionDataNew) { // Indicate that information is no longer usable posControl.flags.horizontalPositionDataConsumed = true; @@ -731,6 +855,10 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) // If we have new position data - update velocity and acceleration controllers if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { + if (navigationVtolMcProtectionApplyCapture(navGetCurrentStateFlags())) { + setMulticopterStopPosition(); + } + // Get max speed for current NAV mode float maxSpeed = getActiveSpeed(); updatePositionVelocityController_MC(maxSpeed); @@ -770,7 +898,7 @@ void updateBaroAltitudeRate(float newBaroAltRate) baroAltRate = newBaroAltRate; } -static bool isLandingGbumpDetected(timeMs_t currentTimeMs) +static bool isLandingGbumpDetected(timeMs_t currentTimeMs, bool landingBumpAllowed) { /* Detection based on G bump at touchdown, falling Baro altitude and throttle below hover. * G bump trigger: > 2g then falling back below 1g in < 0.1s. @@ -779,6 +907,11 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs) static timeMs_t gSpikeDetectTimeMs = 0; + if (!landingBumpAllowed) { + gSpikeDetectTimeMs = 0; + return false; + } + if (!gSpikeDetectTimeMs && acc.accADCf[Z] > 2.0f && baroAltRate < 0.0f) { gSpikeDetectTimeMs = currentTimeMs; } else if (gSpikeDetectTimeMs) { @@ -786,7 +919,11 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs) if (acc.accADCf[Z] < 1.0f && baroAltRate < -200.0f) { const uint16_t idleThrottle = getThrottleIdleValue(); const uint16_t hoverThrottleRange = currentBatteryProfile->nav.mc.hover_throttle - idleThrottle; - return rcCommand[THROTTLE] < idleThrottle + ((navigationRequiresAutoThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange); + const bool detected = rcCommand[THROTTLE] < idleThrottle + ((navigationRequiresAutoThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange); + if (detected) { + gSpikeDetectTimeMs = 0; + } + return detected; } } else if (acc.accADCf[Z] <= 1.0f) { gSpikeDetectTimeMs = 0; @@ -823,6 +960,11 @@ bool isMulticopterLandingDetected(void) DEBUG_SET(DEBUG_LANDING, 3, averageAbsGyroRates() * 100); const timeMs_t currentTimeMs = millis(); + const uint32_t navStateFlags = navGetCurrentStateFlags(); + const float verticalSpeed = navGetCurrentActualPositionAndVelocity()->vel.z; + const float absVerticalSpeed = fabsf(verticalSpeed); + const float finalDescentDemandLimit = navConfig()->general.land_minalt_vspd + MC_LAND_DETECT_DESCENT_DEMAND_MARGIN; + bool gBumpDetected = false; #if defined(USE_BARO) if (sensors(SENSOR_BARO)) { @@ -838,8 +980,20 @@ bool isMulticopterLandingDetected(void) ((posControl.flags.estPosStatus >= EST_USABLE && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING) || FLIGHT_MODE(FAILSAFE_MODE)); - if (gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs)) { - return true; // Landing flagged immediately if landing bump detected + const bool landingBumpAllowed = vtolMcProtectionLandingBumpAllowed( + navigationVtolMcProtectionIsVtolMcMode(), + navStateFlags & NAV_CTL_LAND, + posControl.flags.estAglStatus == EST_TRUSTED, + posControl.actualState.agl.pos.z, + posControl.flags.estAltStatus >= EST_USABLE, + verticalSpeed, + posControl.desiredState.vel.z, + finalDescentDemandLimit, + VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S); + + gBumpDetected = gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs, landingBumpAllowed); + if (gBumpDetected) { + DEBUG_SET(DEBUG_LANDING, 4, 3); } } #endif @@ -856,7 +1010,9 @@ bool isMulticopterLandingDetected(void) throttleLowCheckAllowed = throttleLowCheckAllowed && posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 50.0f; } - bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) || + bool startCondition = mcLandingProbe.active || + gBumpDetected || + (navStateFlags & (NAV_CTL_LAND | NAV_CTL_EMERG)) || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) || (throttleLowCheckAllowed && throttleStickIsLow()); @@ -864,14 +1020,18 @@ bool isMulticopterLandingDetected(void) if (!startCondition || posControl.flags.resetLandingDetector) { landingDetectorStartedAt = 0; + resetMulticopterLandingProbe(); return posControl.flags.resetLandingDetector = false; } const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f; // check vertical and horizontal velocities are low (cm/s) - bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (MC_LAND_CHECK_VEL_Z_MOVING * sensitivity) && + bool velCondition = absVerticalSpeed < (MC_LAND_CHECK_VEL_Z_MOVING * sensitivity) && posControl.actualState.velXY < (MC_LAND_CHECK_VEL_XY_MOVING * sensitivity); + // Sensitivity may relax the generic velocity check, but auto-disarm still needs + // the aircraft to be nearly stopped vertically when a Z estimate is available. + const bool verticalSpeedSettled = posControl.flags.estAltStatus == EST_NONE || absVerticalSpeed < MC_LAND_DETECT_MAX_VEL_Z; // check gyro rates are low (degs/s) bool gyroCondition = averageAbsGyroRates() < (4.0f * sensitivity); DEBUG_SET(DEBUG_LANDING, 2, velCondition); @@ -879,7 +1039,13 @@ bool isMulticopterLandingDetected(void) bool possibleLandingDetected = false; - if (navGetCurrentStateFlags() & NAV_CTL_LAND) { + if (mcLandingProbe.active) { + DEBUG_SET(DEBUG_LANDING, 4, 4); + possibleLandingDetected = true; + } else if (gBumpDetected) { + DEBUG_SET(DEBUG_LANDING, 4, 3); + possibleLandingDetected = true; + } else if (navStateFlags & NAV_CTL_LAND) { // We have likely landed if throttle is 40 units below average descend throttle // We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed // from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core) @@ -904,14 +1070,19 @@ bool isMulticopterLandingDetected(void) landingThrSum += rcCommandAdjustedThrottle; isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE); - possibleLandingDetected = isAtMinimalThrust && velCondition; + const bool descentDemandSettled = posControl.desiredState.vel.z > -finalDescentDemandLimit; + const int16_t landingThrottleLimit = (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()) / 2; + const bool throttleLowEnough = rcCommandAdjustedThrottle < landingThrottleLimit; + + possibleLandingDetected = isAtMinimalThrust && throttleLowEnough && + velCondition && verticalSpeedSettled && descentDemandSettled; DEBUG_SET(DEBUG_LANDING, 6, rcCommandAdjustedThrottle); DEBUG_SET(DEBUG_LANDING, 7, landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE); } else { // non autonomous and emergency landing DEBUG_SET(DEBUG_LANDING, 4, 2); if (landingDetectorStartedAt) { - possibleLandingDetected = velCondition && gyroCondition; + possibleLandingDetected = velCondition && verticalSpeedSettled && gyroCondition; } else { landingDetectorStartedAt = currentTimeMs; return false; @@ -932,11 +1103,28 @@ bool isMulticopterLandingDetected(void) * Fixed time increased if Z velocity invalid to provide extra safety margin against false triggers */ const uint16_t safetyTime = posControl.flags.estAltStatus == EST_NONE ? 5000 : 1000; timeMs_t safetyTimeDelay = safetyTime + navConfig()->general.auto_disarm_delay; - return currentTimeMs - landingDetectorStartedAt > safetyTimeDelay; + if (gBumpDetected || currentTimeMs - landingDetectorStartedAt > safetyTimeDelay) { + if (!navigationVtolMcProtectionIsVtolMcMode()) { + resetMulticopterLandingProbe(); + return true; + } + + bool probeAborted = false; + const bool probeConfirmed = updateMulticopterLandingProbe(currentTimeMs, &probeAborted); + if (probeAborted) { + landingDetectorStartedAt = currentTimeMs; + } + + return probeConfirmed; + } } else { landingDetectorStartedAt = currentTimeMs; + resetMulticopterLandingProbe(); return false; } + + resetMulticopterLandingProbe(); + return false; } bool isMulticopterThrottleAboveMidHover(void) @@ -1037,4 +1225,4 @@ void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlag if (navStateFlags & NAV_CTL_YAW) applyMulticopterHeadingController(); } -} \ No newline at end of file +} diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 2e305289e83..862ea84d057 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -41,6 +41,11 @@ #define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s #define MC_LAND_CHECK_VEL_Z_MOVING 100.0f // cm/s +// A landed multicopter should have near-zero vertical speed. Keep this independent +// from nav_land_detect_sensitivity so higher sensitivity cannot disarm during descent. +#define MC_LAND_DETECT_MAX_VEL_Z 50.0f // cm/s +// Only allow autonomous land detection near the configured final slow-descent phase. +#define MC_LAND_DETECT_DESCENT_DEMAND_MARGIN 25.0f // cm/s #define MC_LAND_THR_STABILISE_DELAY 1 // seconds #define MC_LAND_DESCEND_THROTTLE 40 // RC pwm units (us) #define MC_LAND_SAFE_SURFACE 5.0f // cm @@ -184,8 +189,20 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3, NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4, +#ifdef USE_AUTO_TRANSITION + // Only valid while MixerAT is in progress. The state-specific slots are + // intentionally reused by other FSM states. + NAV_FSM_EVENT_MIXERAT_MISSION_ADVANCE = NAV_FSM_EVENT_STATE_SPECIFIC_1, + NAV_FSM_EVENT_MIXERAT_MISSION_CAPTURE = NAV_FSM_EVENT_STATE_SPECIFIC_2, +#endif NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5, +#ifdef USE_AUTO_TRANSITION + // Capture must also accept MSP waypoint jumps, which already use state + // specific slot 4. Keep resume distinct so those events cannot alias. + NAV_FSM_EVENT_MIXERAT_MISSION_RESUME, +#endif + NAV_FSM_EVENT_COUNT, } navigationFSMEvent_t; @@ -256,7 +273,8 @@ typedef enum { NAV_PERSISTENT_ID_SEND_TO_INITALIZE = 49, NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES = 50, - NAV_PERSISTENT_ID_SEND_TO_FINISHED = 51 + NAV_PERSISTENT_ID_SEND_TO_FINISHED = 51, + NAV_PERSISTENT_ID_MIXERAT_MISSION_CAPTURE = 52, } navigationPersistentId_e; typedef enum { @@ -315,6 +333,7 @@ typedef enum { NAV_STATE_MIXERAT_INITIALIZE, NAV_STATE_MIXERAT_IN_PROGRESS, NAV_STATE_MIXERAT_ABORT, + NAV_STATE_MIXERAT_MISSION_CAPTURE, NAV_STATE_SEND_TO_INITALIZE, NAV_STATE_SEND_TO_IN_PROGESS, @@ -509,6 +528,9 @@ typedef struct { uint16_t wpReachedSeq; // Last reached mission item sequence relative to startWpIndex bool wpReachedNotificationPending; bool wpAltitudeReached; // WP altitude achieved + bool wpAltitudeEnforceActive; // WP entered altitude enforcement window + bool wpAltitudeEnforceFromStart; // First MC WP is climbing before horizontal travel + fpVector3_t wpAltitudeEnforceHoldPos; // XY position captured before first-WP climb #ifdef USE_FW_AUTOLAND /* Fixedwing autoland */ @@ -586,6 +608,8 @@ void resetMulticopterAltitudeController(void); void resetMulticopterPositionController(void); void resetMulticopterHeadingController(void); void resetMulticopterBrakingMode(void); +bool navigationMulticopterBrakingActive(void); +bool navigationMulticopterBrakingBoostActive(void); bool adjustMulticopterAltitudeFromRCInput(void); bool adjustMulticopterHeadingFromRCInput(void); diff --git a/src/main/navigation/navigation_vtol_mc_protection.c b/src/main/navigation/navigation_vtol_mc_protection.c new file mode 100644 index 00000000000..2c4b1c31485 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mc_protection.c @@ -0,0 +1,545 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" + +#include "fc/config.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/mixer_profile.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" +#include "navigation/navigation_vtol_mc_protection.h" + +#ifdef USE_AUTO_TRANSITION + +typedef enum { + VTOL_MC_PROTECT_FLAG_CONFIGURED = 1 << 0, + VTOL_MC_PROTECT_FLAG_VTOL_MC = 1 << 1, + VTOL_MC_PROTECT_FLAG_NAV_ACTIVE = 1 << 2, + VTOL_MC_PROTECT_FLAG_STABILIZED_ACTIVE = 1 << 3, + VTOL_MC_PROTECT_FLAG_CAPTURE_ACTIVE = 1 << 4, + VTOL_MC_PROTECT_FLAG_LANDING_SETTLE = 1 << 5, + VTOL_MC_PROTECT_FLAG_BAILOUT_ACTIVE = 1 << 6, + VTOL_MC_PROTECT_FLAG_RESERVE_SHRUNK = 1 << 7, + VTOL_MC_PROTECT_FLAG_SOFT_ALTITUDE = 1 << 8, + VTOL_MC_PROTECT_FLAG_COMMAND_SHAPED = 1 << 9, + VTOL_MC_PROTECT_FLAG_VELOCITY_FALLBACK = 1 << 10, +} vtolMcProtectionDebugFlags_e; + +typedef struct vtolMcProtectionRuntimeState_s { + vtolMcProtectionSettleState_t captureSettle; + vtolMcProtectionSettleState_t landingSettle; + vtolMcProtectionSettleState_t bailoutSettle; + bool captureActive; + bool softAltitudeActive; + bool landingSettleActive; + bool landingSettleApproved; + bool bailoutActive; + bool reserveShrunk; + bool stabilizedCommandShaped; + bool velocityFallbackActive; + timeMs_t bailoutStartMs; + int16_t bailoutStartThrottle; + uint16_t commandScalePermille; + int16_t safeThrottleMin; + int16_t safeThrottleMax; + int16_t protectedThrottle; +} vtolMcProtectionRuntimeState_t; + +static vtolMcProtectionRuntimeState_t vtolMcProtection = { + .commandScalePermille = 1000, +}; + +static bool navigationVtolMcProtectionConfigured(void) +{ + return (vtolMcProtectionMode_e)systemConfig()->vtolMcProtectionMode != VTOL_MC_PROTECTION_OFF; +} + +static bool navigationVtolMcProtectionPairedAirplaneProfileConfigured(void) +{ + for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) { + if (i != currentMixerProfileIndex && mixerConfigByIndex(i)->platformType == PLATFORM_AIRPLANE) { + return true; + } + } + + return platformTypeConfigured(PLATFORM_AIRPLANE); +} + +bool navigationVtolMcProtectionIsVtolMcMode(void) +{ + return vtolMcProtectionDetectVtolMcMode( + STATE(MULTIROTOR), + isMultirotorTypePlatform(currentMixerConfig.platformType), + navigationVtolMcProtectionPairedAirplaneProfileConfigured()); +} + +bool navigationVtolMcProtectionIsNavActive(void) +{ + return navigationVtolMcProtectionConfigured() && + ARMING_FLAG(ARMED) && + navigationVtolMcProtectionIsVtolMcMode(); +} + +bool navigationVtolMcProtectionVelocityUsable(void) +{ + return posControl.flags.estVelStatus == EST_TRUSTED; +} + +static bool navigationVtolMcProtectionVerticalVelocityUsable(void) +{ + return posControl.flags.estAltStatus >= EST_USABLE; +} + +uint16_t navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(void) +{ + const uint16_t absRoll = ABS(attitude.values.roll); + const uint16_t absPitch = ABS(attitude.values.pitch); + return absRoll > absPitch ? absRoll : absPitch; +} + +static uint16_t navigationVtolMcProtectionHorizontalSettleLimitCmS(void) +{ +#ifdef USE_MR_BRAKING_MODE + return vtolMcProtectionHorizontalSettleSpeedCmS(navConfig()->mc.braking_disengage_speed); +#else + return vtolMcProtectionHorizontalSettleSpeedCmS(0); +#endif +} + +static uint16_t navigationVtolMcProtectionVerticalSettleLimitCmS(void) +{ + return vtolMcProtectionVerticalSettleSpeedCmS(navConfig()->general.land_minalt_vspd); +} + +static uint16_t navigationVtolMcProtectionAttitudeSettleLimitDeciDeg(void) +{ + return vtolMcProtectionSettleAttitudeLimitDeciDeg(navConfig()->mc.max_bank_angle); +} + +static uint16_t navigationVtolMcProtectionBailoutLimitDeciDeg(void) +{ + return vtolMcProtectionBailoutAngleLimitDeciDeg(navConfig()->mc.max_bank_angle); +} + +static bool navigationVtolMcProtectionSettleConditionsMet(uint16_t *settleTimeMs) +{ + const bool horizontalVelocityUsable = navigationVtolMcProtectionVelocityUsable(); + const bool verticalVelocityUsable = navigationVtolMcProtectionVerticalVelocityUsable(); + + vtolMcProtection.velocityFallbackActive = vtolMcProtectionUsingVelocityFallback(horizontalVelocityUsable, verticalVelocityUsable); + *settleTimeMs = vtolMcProtectionSettleTimeMs(horizontalVelocityUsable); + + return vtolMcProtectionSettleConditionsMetWithFallback( + horizontalVelocityUsable, + verticalVelocityUsable, + posControl.actualState.velXY, + navGetCurrentActualPositionAndVelocity()->vel.z, + navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(), + navigationVtolMcProtectionHorizontalSettleLimitCmS(), + navigationVtolMcProtectionVerticalSettleLimitCmS(), + navigationVtolMcProtectionAttitudeSettleLimitDeciDeg()); +} + +static void navigationVtolMcProtectionPublishDebug(void) +{ + uint32_t flags = 0; + + if (navigationVtolMcProtectionConfigured()) { + flags |= VTOL_MC_PROTECT_FLAG_CONFIGURED; + } + if (navigationVtolMcProtectionIsVtolMcMode()) { + flags |= VTOL_MC_PROTECT_FLAG_VTOL_MC; + } + if (navigationVtolMcProtectionIsNavActive()) { + flags |= VTOL_MC_PROTECT_FLAG_NAV_ACTIVE; + } + if ((vtolMcProtectionMode_e)systemConfig()->vtolMcProtectionMode == VTOL_MC_PROTECTION_NAV_AND_STABILIZED && + ARMING_FLAG(ARMED) && navigationVtolMcProtectionIsVtolMcMode() && + (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + flags |= VTOL_MC_PROTECT_FLAG_STABILIZED_ACTIVE; + } + if (vtolMcProtection.captureActive) { + flags |= VTOL_MC_PROTECT_FLAG_CAPTURE_ACTIVE; + } + if (vtolMcProtection.landingSettleActive) { + flags |= VTOL_MC_PROTECT_FLAG_LANDING_SETTLE; + } + if (vtolMcProtection.bailoutActive) { + flags |= VTOL_MC_PROTECT_FLAG_BAILOUT_ACTIVE; + } + if (vtolMcProtection.reserveShrunk) { + flags |= VTOL_MC_PROTECT_FLAG_RESERVE_SHRUNK; + } + if (vtolMcProtection.softAltitudeActive) { + flags |= VTOL_MC_PROTECT_FLAG_SOFT_ALTITUDE; + } + if (vtolMcProtection.stabilizedCommandShaped) { + flags |= VTOL_MC_PROTECT_FLAG_COMMAND_SHAPED; + } + if (vtolMcProtection.velocityFallbackActive && + (vtolMcProtection.captureActive || vtolMcProtection.landingSettleActive || vtolMcProtection.bailoutActive)) { + flags |= VTOL_MC_PROTECT_FLAG_VELOCITY_FALLBACK; + } + + uint16_t debugProgress = 1000; + if (vtolMcProtection.captureActive) { + debugProgress = vtolMcProtection.captureSettle.elapsedMs; + } else if (vtolMcProtection.landingSettleActive) { + debugProgress = vtolMcProtection.landingSettle.elapsedMs; + } else if (vtolMcProtection.bailoutActive) { + debugProgress = vtolMcProtection.bailoutSettle.elapsedMs; + } else if (vtolMcProtection.stabilizedCommandShaped) { + debugProgress = vtolMcProtection.commandScalePermille; + } + + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 0, (int32_t)flags); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 1, vtolMcProtection.safeThrottleMin); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 2, vtolMcProtection.safeThrottleMax); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 3, vtolMcProtection.protectedThrottle); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 4, (int32_t)lrintf(posControl.actualState.velXY)); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 5, (int32_t)lrintf(navGetCurrentActualPositionAndVelocity()->vel.z)); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 6, navigationVtolMcProtectionMaxAbsAttitudeDeciDeg()); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 7, debugProgress); +} + +void navigationVtolMcProtectionResetTransientStates(void) +{ + vtolMcProtection.captureSettle.stableSinceMs = 0; + vtolMcProtection.captureSettle.elapsedMs = 0; + vtolMcProtection.landingSettle.stableSinceMs = 0; + vtolMcProtection.landingSettle.elapsedMs = 0; + vtolMcProtection.bailoutSettle.stableSinceMs = 0; + vtolMcProtection.bailoutSettle.elapsedMs = 0; + vtolMcProtection.captureActive = false; + vtolMcProtection.softAltitudeActive = false; + vtolMcProtection.landingSettleActive = false; + vtolMcProtection.landingSettleApproved = false; + vtolMcProtection.bailoutActive = false; + vtolMcProtection.reserveShrunk = false; + vtolMcProtection.stabilizedCommandShaped = false; + vtolMcProtection.velocityFallbackActive = false; + vtolMcProtection.commandScalePermille = 1000; + vtolMcProtection.protectedThrottle = 0; +} + +void navigationVtolMcProtectionResetLandingSettle(void) +{ + vtolMcProtection.landingSettle.stableSinceMs = 0; + vtolMcProtection.landingSettle.elapsedMs = 0; + vtolMcProtection.landingSettleActive = false; + vtolMcProtection.landingSettleApproved = false; +} + +void navigationVtolMcProtectionResetCapture(void) +{ + vtolMcProtection.captureSettle.stableSinceMs = 0; + vtolMcProtection.captureSettle.elapsedMs = 0; + vtolMcProtection.captureActive = false; + navigationVtolMcProtectionPublishDebug(); +} + +vtolMcProtectionThrottleBounds_t navigationVtolMcProtectionGetThrottleBounds(const int16_t idleThrottle, const int16_t hoverThrottle, const int16_t maxThrottle) +{ + const bool active = navigationVtolMcProtectionIsNavActive() && navigationRequiresAutoThrottleMode(); + vtolMcProtectionThrottleBounds_t bounds = vtolMcProtectionComputeThrottleBounds( + active, + idleThrottle, + hoverThrottle, + maxThrottle, + systemConfig()->vtolMcThrReservePercent); + + vtolMcProtection.safeThrottleMin = bounds.min; + vtolMcProtection.safeThrottleMax = bounds.max; + vtolMcProtection.reserveShrunk = bounds.reserveShrunk; + + if (!active) { + vtolMcProtection.bailoutActive = false; + vtolMcProtection.softAltitudeActive = false; + vtolMcProtection.velocityFallbackActive = false; + } + + navigationVtolMcProtectionPublishDebug(); + return bounds; +} + +static bool navigationVtolMcProtectionBailoutEntryCondition(void) +{ + return navigationVtolMcProtectionIsNavActive() && + navigationRequiresAutoThrottleMode() && + navigationVtolMcProtectionMaxAbsAttitudeDeciDeg() >= navigationVtolMcProtectionBailoutLimitDeciDeg(); +} + +bool navigationVtolMcProtectionShouldFreezeAltitudeIntegrator(void) +{ + return vtolMcProtection.bailoutActive || + vtolMcProtection.softAltitudeActive || + navigationVtolMcProtectionBailoutEntryCondition(); +} + +static int16_t navigationVtolMcProtectionBailoutTargetThrottle(const int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, const int16_t hoverThrottle) +{ + int16_t targetThrottle = constrain(hoverThrottle, bounds->min, bounds->max); + const float verticalSpeedCmS = navGetCurrentActualPositionAndVelocity()->vel.z; + + if (verticalSpeedCmS < -(float)navigationVtolMcProtectionVerticalSettleLimitCmS() && targetThrottle < requestedThrottle) { + targetThrottle = requestedThrottle; + } + + return constrain(targetThrottle, bounds->min, bounds->max); +} + +int16_t navigationVtolMcProtectionApplyBailoutThrottle(const int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, const int16_t hoverThrottle) +{ + const bool entryCondition = navigationVtolMcProtectionBailoutEntryCondition(); + const timeMs_t nowMs = millis(); + + if (!entryCondition && vtolMcProtection.bailoutActive) { + uint16_t settleTimeMs; + const bool settleConditionsMet = navigationVtolMcProtectionSettleConditionsMet(&settleTimeMs); + const bool readyToExit = vtolMcProtectionUpdateSettleState( + &vtolMcProtection.bailoutSettle, + settleConditionsMet, + settleTimeMs, + nowMs); + + if (readyToExit) { + vtolMcProtection.bailoutActive = false; + vtolMcProtection.bailoutSettle.stableSinceMs = 0; + vtolMcProtection.bailoutSettle.elapsedMs = 0; + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + vtolMcProtection.protectedThrottle = requestedThrottle; + navigationVtolMcProtectionPublishDebug(); + return requestedThrottle; + } + } + + if (!entryCondition && !vtolMcProtection.bailoutActive) { + vtolMcProtection.protectedThrottle = requestedThrottle; + navigationVtolMcProtectionPublishDebug(); + return requestedThrottle; + } + + if (!vtolMcProtection.bailoutActive) { + vtolMcProtection.bailoutActive = true; + vtolMcProtection.bailoutStartMs = nowMs; + vtolMcProtection.bailoutStartThrottle = requestedThrottle; + vtolMcProtection.bailoutSettle.stableSinceMs = 0; + vtolMcProtection.bailoutSettle.elapsedMs = 0; + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + } + + const int16_t targetThrottle = navigationVtolMcProtectionBailoutTargetThrottle(requestedThrottle, bounds, hoverThrottle); + const uint16_t rampTimeMs = VTOL_MC_SETTLE_TIME_MS > VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS ? + VTOL_MC_SETTLE_TIME_MS : + VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS; + const float progress = constrainf((nowMs - vtolMcProtection.bailoutStartMs) / (float)rampTimeMs, 0.0f, 1.0f); + const int16_t protectedThrottle = lrintf(vtolMcProtection.bailoutStartThrottle + + (targetThrottle - vtolMcProtection.bailoutStartThrottle) * progress); + + vtolMcProtection.protectedThrottle = constrain(protectedThrottle, bounds->min, bounds->max); + navigationVtolMcProtectionPublishDebug(); + return vtolMcProtection.protectedThrottle; +} + +bool navigationVtolMcProtectionApplyCapture(const uint32_t navStateFlags) +{ + const bool captureAllowed = vtolMcProtectionPositionCaptureAllowed( + navigationVtolMcProtectionIsNavActive(), + (navStateFlags & NAV_CTL_POS) && (navStateFlags & NAV_CTL_HOLD), + (navStateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP | NAV_CTL_LAND)) != 0, + posControl.flags.isAdjustingPosition); + + if (!captureAllowed) { + navigationVtolMcProtectionResetCapture(); + navigationVtolMcProtectionPublishDebug(); + return false; + } + + uint16_t settleTimeMs; + const bool settleConditionsMet = navigationVtolMcProtectionSettleConditionsMet(&settleTimeMs); + const bool ready = vtolMcProtectionUpdateSettleState( + &vtolMcProtection.captureSettle, + settleConditionsMet, + settleTimeMs, + millis()); + + const bool captureWasActive = vtolMcProtection.captureActive; + vtolMcProtection.captureActive = !ready; + navigationVtolMcProtectionPublishDebug(); + return vtolMcProtectionCaptureShouldSetTarget(captureWasActive, vtolMcProtection.captureActive); +} + +bool navigationVtolMcProtectionApplySoftAltitudeCapture(const uint32_t navStateFlags) +{ + const bool holdOrTransitionState = ((navStateFlags & NAV_CTL_POS) && (navStateFlags & NAV_CTL_HOLD)) || + (navStateFlags & NAV_MIXERAT); + const bool shouldRelaxAltitude = vtolMcProtectionShouldRelaxAltitude( + navigationVtolMcProtectionIsNavActive(), + navigationRequiresAutoThrottleMode(), + navigationVtolMcProtectionVelocityUsable(), + holdOrTransitionState, + navStateFlags & NAV_CTL_LAND, + navStateFlags & NAV_CTL_EMERG, + posControl.flags.isAdjustingAltitude, + posControl.actualState.velXY, + navigationVtolMcProtectionHorizontalSettleLimitCmS(), + vtolMcProtection.captureActive); + + vtolMcProtection.softAltitudeActive = shouldRelaxAltitude; + + if (shouldRelaxAltitude) { + // While a winged VTOL is bleeding horizontal speed, avoid chasing a stale + // altitude lock point; stabilize vertical rate and re-lock after settling. + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + } + + navigationVtolMcProtectionPublishDebug(); + return shouldRelaxAltitude; +} + +static bool navigationVtolMcProtectionLandingSettleConditionsMetInternal(const fpVector3_t *landingPos, uint16_t *settleTimeMs) +{ + const uint16_t landingCaptureRadiusCm = vtolMcProtectionLandingCaptureRadiusCm(navConfig()->general.waypoint_radius); + const bool insideLandingRadius = calculateDistanceToDestination(landingPos) <= landingCaptureRadiusCm; + + *settleTimeMs = 0; + const bool settleConditionsMet = navigationVtolMcProtectionSettleConditionsMet(settleTimeMs); + + return insideLandingRadius && settleConditionsMet; +} + +bool navigationVtolMcProtectionLandingSettleConditionsMet(const fpVector3_t *landingPos) +{ + if (!navigationVtolMcProtectionIsNavActive()) { + navigationVtolMcProtectionResetLandingSettle(); + navigationVtolMcProtectionPublishDebug(); + return true; + } + + uint16_t settleTimeMs; + const bool conditionsMet = navigationVtolMcProtectionLandingSettleConditionsMetInternal(landingPos, &settleTimeMs); + vtolMcProtection.landingSettleActive = true; + navigationVtolMcProtectionPublishDebug(); + return conditionsMet; +} + +bool navigationVtolMcProtectionLandingSettleReady(const fpVector3_t *landingPos) +{ + if (!navigationVtolMcProtectionIsNavActive()) { + navigationVtolMcProtectionResetLandingSettle(); + navigationVtolMcProtectionPublishDebug(); + return true; + } + + if (!vtolMcProtection.landingSettleApproved) { + uint16_t settleTimeMs; + const bool settleConditionsMet = navigationVtolMcProtectionLandingSettleConditionsMetInternal(landingPos, &settleTimeMs); + vtolMcProtection.landingSettleApproved = vtolMcProtectionUpdateSettleState( + &vtolMcProtection.landingSettle, + settleConditionsMet, + settleTimeMs, + millis()); + } + + vtolMcProtection.landingSettleActive = !vtolMcProtection.landingSettleApproved; + navigationVtolMcProtectionPublishDebug(); + return vtolMcProtection.landingSettleApproved; +} + +bool navigationVtolMcProtectionLandingDescentNeedsResettle(void) +{ + const bool needsResettle = vtolMcProtectionLandingDescentNeedsResettle( + navigationVtolMcProtectionIsNavActive(), + vtolMcProtection.landingSettleApproved, + navigationVtolMcProtectionVelocityUsable(), + posControl.actualState.velXY, + navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(), + navigationVtolMcProtectionHorizontalSettleLimitCmS(), + navigationVtolMcProtectionAttitudeSettleLimitDeciDeg()); + + if (needsResettle) { + navigationVtolMcProtectionResetLandingSettle(); + vtolMcProtection.landingSettleActive = true; + } + + navigationVtolMcProtectionPublishDebug(); + return needsResettle; +} + +void navigationVtolMcProtectionApplyStabilizedCommandShaping(int16_t *rollCommand, int16_t *pitchCommand, int16_t *yawCommand) +{ + vtolMcProtection.stabilizedCommandShaped = false; + vtolMcProtection.commandScalePermille = 1000; + + if ((vtolMcProtectionMode_e)systemConfig()->vtolMcProtectionMode != VTOL_MC_PROTECTION_NAV_AND_STABILIZED || + !ARMING_FLAG(ARMED) || + !navigationVtolMcProtectionIsVtolMcMode() || + !(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) || + !navigationVtolMcProtectionVelocityUsable()) { + navigationVtolMcProtectionPublishDebug(); + return; + } + + const float commandScale = vtolMcProtectionCommandScaleForSpeed(posControl.actualState.velXY); + vtolMcProtection.commandScalePermille = vtolMcProtectionCommandScalePermille(commandScale); + + if (commandScale >= 0.999f) { + navigationVtolMcProtectionPublishDebug(); + return; + } + + const int16_t originalRoll = *rollCommand; + const int16_t originalPitch = *pitchCommand; + const int16_t originalYaw = *yawCommand; + + *rollCommand = vtolMcProtectionApplyCommandScale(*rollCommand, commandScale); + *pitchCommand = vtolMcProtectionApplyCommandScale(*pitchCommand, commandScale); + *yawCommand = vtolMcProtectionApplyCommandScale(*yawCommand, commandScale); + + vtolMcProtection.stabilizedCommandShaped = *rollCommand != originalRoll || + *pitchCommand != originalPitch || + *yawCommand != originalYaw; + navigationVtolMcProtectionPublishDebug(); +} + +void navigationVtolMcProtectionPublishThrottleDebug(const vtolMcProtectionThrottleBounds_t *bounds, const int16_t protectedThrottle) +{ + vtolMcProtection.safeThrottleMin = bounds->min; + vtolMcProtection.safeThrottleMax = bounds->max; + vtolMcProtection.reserveShrunk = bounds->reserveShrunk; + vtolMcProtection.protectedThrottle = protectedThrottle; + navigationVtolMcProtectionPublishDebug(); +} + +#endif diff --git a/src/main/navigation/navigation_vtol_mc_protection.h b/src/main/navigation/navigation_vtol_mc_protection.h new file mode 100644 index 00000000000..7e1e76ae8e0 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mc_protection.h @@ -0,0 +1,101 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#include "platform.h" + +#include "common/time.h" +#include "common/vector.h" + +#include "navigation/navigation_vtol_mc_protection_logic.h" + +#ifdef USE_AUTO_TRANSITION +bool navigationVtolMcProtectionIsVtolMcMode(void); +bool navigationVtolMcProtectionIsNavActive(void); +bool navigationVtolMcProtectionVelocityUsable(void); +uint16_t navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(void); + +vtolMcProtectionThrottleBounds_t navigationVtolMcProtectionGetThrottleBounds(int16_t idleThrottle, int16_t hoverThrottle, int16_t maxThrottle); +bool navigationVtolMcProtectionShouldFreezeAltitudeIntegrator(void); +int16_t navigationVtolMcProtectionApplyBailoutThrottle(int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, int16_t hoverThrottle); + +bool navigationVtolMcProtectionApplyCapture(uint32_t navStateFlags); +void navigationVtolMcProtectionResetCapture(void); +bool navigationVtolMcProtectionApplySoftAltitudeCapture(uint32_t navStateFlags); +bool navigationVtolMcProtectionLandingSettleConditionsMet(const fpVector3_t *landingPos); +bool navigationVtolMcProtectionLandingSettleReady(const fpVector3_t *landingPos); +bool navigationVtolMcProtectionLandingDescentNeedsResettle(void); +void navigationVtolMcProtectionResetLandingSettle(void); +void navigationVtolMcProtectionApplyStabilizedCommandShaping(int16_t *rollCommand, int16_t *pitchCommand, int16_t *yawCommand); +void navigationVtolMcProtectionPublishThrottleDebug(const vtolMcProtectionThrottleBounds_t *bounds, int16_t protectedThrottle); +void navigationVtolMcProtectionResetTransientStates(void); +#else +static inline bool navigationVtolMcProtectionIsVtolMcMode(void) { return false; } +static inline bool navigationVtolMcProtectionIsNavActive(void) { return false; } +static inline bool navigationVtolMcProtectionVelocityUsable(void) { return false; } +static inline uint16_t navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(void) { return 0; } +static inline vtolMcProtectionThrottleBounds_t navigationVtolMcProtectionGetThrottleBounds(int16_t idleThrottle, int16_t hoverThrottle, int16_t maxThrottle) +{ + return vtolMcProtectionComputeThrottleBounds(false, idleThrottle, hoverThrottle, maxThrottle, 0); +} +static inline bool navigationVtolMcProtectionShouldFreezeAltitudeIntegrator(void) { return false; } +static inline int16_t navigationVtolMcProtectionApplyBailoutThrottle(int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, int16_t hoverThrottle) +{ + (void)bounds; + (void)hoverThrottle; + return requestedThrottle; +} +static inline bool navigationVtolMcProtectionApplyCapture(uint32_t navStateFlags) +{ + (void)navStateFlags; + return false; +} +static inline void navigationVtolMcProtectionResetCapture(void) {} +static inline bool navigationVtolMcProtectionApplySoftAltitudeCapture(uint32_t navStateFlags) +{ + (void)navStateFlags; + return false; +} +static inline bool navigationVtolMcProtectionLandingSettleConditionsMet(const fpVector3_t *landingPos) +{ + (void)landingPos; + return true; +} +static inline bool navigationVtolMcProtectionLandingSettleReady(const fpVector3_t *landingPos) +{ + (void)landingPos; + return true; +} +static inline bool navigationVtolMcProtectionLandingDescentNeedsResettle(void) { return false; } +static inline void navigationVtolMcProtectionResetLandingSettle(void) {} +static inline void navigationVtolMcProtectionApplyStabilizedCommandShaping(int16_t *rollCommand, int16_t *pitchCommand, int16_t *yawCommand) +{ + (void)rollCommand; + (void)pitchCommand; + (void)yawCommand; +} +static inline void navigationVtolMcProtectionPublishThrottleDebug(const vtolMcProtectionThrottleBounds_t *bounds, int16_t protectedThrottle) +{ + (void)bounds; + (void)protectedThrottle; +} +static inline void navigationVtolMcProtectionResetTransientStates(void) {} +#endif diff --git a/src/main/navigation/navigation_vtol_mc_protection_logic.h b/src/main/navigation/navigation_vtol_mc_protection_logic.h new file mode 100644 index 00000000000..13b71dfdb56 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mc_protection_logic.h @@ -0,0 +1,422 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#include "common/time.h" + +#define VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM 100 +#define VTOL_MC_LANDING_RESETTLE_SPEED_MULTIPLIER 2 +#define VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S 100 +#define VTOL_MC_SETTLE_ATTITUDE_CAP_DEG 20 +#define VTOL_MC_BAILOUT_ANGLE_EXTRA_DEG 15 +#define VTOL_MC_BAILOUT_ANGLE_MIN_DEG 45 +#define VTOL_MC_BAILOUT_ANGLE_MAX_DEG 60 +#define VTOL_MC_COMMAND_SHAPE_START_CM_S 300 +#define VTOL_MC_COMMAND_SHAPE_FULL_CM_S 800 +#define VTOL_MC_COMMAND_SHAPE_MIN_SCALE 0.5f +#define VTOL_MC_SETTLE_TIME_MS 1000 +#define VTOL_MC_FALLBACK_SETTLE_TIME_MS 3000 +#define VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS 500 +#define VTOL_MC_DEFAULT_BRAKING_DISENGAGE_CM_S 75 +#define VTOL_MC_LANDING_BUMP_SAFE_AGL_CM 50.0f +#define VTOL_MC_LANDING_PROBE_RAMP_TIME_MS 300 +#define VTOL_MC_LANDING_PROBE_CONFIRM_TIME_MS 700 +#define VTOL_MC_LANDING_PROBE_THROTTLE_DROP_PERCENT 10 +#define VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MIN 40 +#define VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MAX 100 +#define VTOL_MC_LANDING_PROBE_AGL_DROP_CM 8.0f +#define VTOL_MC_LANDING_PROBE_VEL_DELTA_CM_S 20.0f +#define VTOL_MC_LANDING_PROBE_VEL_ABS_CM_S 80.0f +#define VTOL_MC_LANDING_PROBE_LOW_G_THRESHOLD 0.85f + +typedef struct vtolMcProtectionThrottleBounds_s { + int16_t min; + int16_t max; + bool reserveShrunk; +} vtolMcProtectionThrottleBounds_t; + +typedef struct vtolMcProtectionSettleState_s { + timeMs_t stableSinceMs; + uint16_t elapsedMs; +} vtolMcProtectionSettleState_t; + +static inline bool vtolMcProtectionDetectVtolMcMode( + const bool currentStateIsMultirotor, + const bool currentPlatformIsMultirotorType, + const bool pairedAirplaneProfileConfigured) +{ + return currentStateIsMultirotor && currentPlatformIsMultirotorType && pairedAirplaneProfileConfigured; +} + +static inline uint16_t vtolMcProtectionLandingCaptureRadiusCm(const uint16_t navWpRadiusCm) +{ + return navWpRadiusCm < VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM ? navWpRadiusCm : VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM; +} + +static inline uint16_t vtolMcProtectionHorizontalSettleSpeedCmS(const uint16_t brakingDisengageSpeedCmS) +{ + return brakingDisengageSpeedCmS > 0 ? brakingDisengageSpeedCmS : VTOL_MC_DEFAULT_BRAKING_DISENGAGE_CM_S; +} + +static inline uint16_t vtolMcProtectionVerticalSettleSpeedCmS(const uint16_t landMinAltVspdCmS) +{ + return landMinAltVspdCmS < VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S ? landMinAltVspdCmS : VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S; +} + +static inline uint16_t vtolMcProtectionSettleAttitudeLimitDeciDeg(const uint8_t navMcBankAngleDeg) +{ + const uint8_t limitedAngleDeg = navMcBankAngleDeg < VTOL_MC_SETTLE_ATTITUDE_CAP_DEG ? navMcBankAngleDeg : VTOL_MC_SETTLE_ATTITUDE_CAP_DEG; + return (uint16_t)limitedAngleDeg * 10U; +} + +static inline bool vtolMcProtectionSuppressesMulticopterBrakingMode(const bool active) +{ + return active; +} + +static inline bool vtolMcProtectionPositionCaptureAllowed( + const bool protectionActive, + const bool positionHoldActive, + const bool navigationOwnsPositionTarget, + const bool pilotPositionInputActive) +{ + // RTH, mission holds and landing own an explicit destination. Re-latching + // a stop target would move that destination. Pilot input must likewise + // retain control of the target in both GPS_ATTI and GPS_CRUISE modes. + return protectionActive && positionHoldActive && + !navigationOwnsPositionTarget && !pilotPositionInputActive; +} + +static inline bool vtolMcProtectionCaptureShouldSetTarget( + const bool captureWasActive, + const bool captureIsActive) +{ + // Capturing the target on every GPS update turns position hold into a + // velocity damper. Only capture the braking target when the phase starts. + return !captureWasActive && captureIsActive; +} + +static inline bool vtolMcProtectionLandingDescentNeedsResettle( + const bool protectionActive, + const bool landingSettleApproved, + const bool horizontalVelocityUsable, + const float horizontalSpeedCmS, + const uint16_t maxAbsAttitudeDeciDeg, + const uint16_t horizontalSettleLimitCmS, + const uint16_t attitudeSettleLimitDeciDeg) +{ + if (!protectionActive || !landingSettleApproved) { + return false; + } + + // A normal descent must not invalidate the gate. Re-settle only for a + // substantial horizontal excursion or a clearly unsafe attitude. + return (horizontalVelocityUsable && + horizontalSpeedCmS > (float)horizontalSettleLimitCmS * VTOL_MC_LANDING_RESETTLE_SPEED_MULTIPLIER) || + maxAbsAttitudeDeciDeg > attitudeSettleLimitDeciDeg; +} + +static inline bool vtolMcProtectionRthLandingYawSettleAssistActive( + const bool active, + const bool landingAllowed, + const bool landingSettleConditionsMet, + const bool headingReached, + const bool fixedWingLegacy) +{ + return active && + landingAllowed && + landingSettleConditionsMet && + !headingReached && + !fixedWingLegacy; +} + +static inline uint16_t vtolMcProtectionBailoutAngleLimitDeciDeg(const uint8_t navMcBankAngleDeg) +{ + uint8_t bailoutAngleDeg = navMcBankAngleDeg + VTOL_MC_BAILOUT_ANGLE_EXTRA_DEG; + + if (bailoutAngleDeg < VTOL_MC_BAILOUT_ANGLE_MIN_DEG) { + bailoutAngleDeg = VTOL_MC_BAILOUT_ANGLE_MIN_DEG; + } else if (bailoutAngleDeg > VTOL_MC_BAILOUT_ANGLE_MAX_DEG) { + bailoutAngleDeg = VTOL_MC_BAILOUT_ANGLE_MAX_DEG; + } + + return (uint16_t)bailoutAngleDeg * 10U; +} + +static inline vtolMcProtectionThrottleBounds_t vtolMcProtectionComputeThrottleBounds( + const bool active, + const int16_t idleThrottle, + const int16_t hoverThrottle, + const int16_t maxThrottle, + const uint8_t reservePercent) +{ + vtolMcProtectionThrottleBounds_t bounds = { + .min = idleThrottle, + .max = maxThrottle, + .reserveShrunk = false, + }; + + if (!active || reservePercent == 0 || maxThrottle <= idleThrottle) { + return bounds; + } + + const int16_t throttleRange = maxThrottle - idleThrottle; + const int16_t requestedReserve = (throttleRange * reservePercent) / 100; + const int16_t lowReserveLimit = hoverThrottle > idleThrottle ? hoverThrottle - idleThrottle : 0; + const int16_t highReserveLimit = maxThrottle > hoverThrottle ? maxThrottle - hoverThrottle : 0; + const int16_t lowReserve = requestedReserve < lowReserveLimit ? requestedReserve : lowReserveLimit; + const int16_t highReserve = requestedReserve < highReserveLimit ? requestedReserve : highReserveLimit; + + bounds.min = idleThrottle + lowReserve; + bounds.max = maxThrottle - highReserve; + bounds.reserveShrunk = lowReserve != requestedReserve || highReserve != requestedReserve; + + return bounds; +} + +static inline bool vtolMcProtectionSettleConditionsMet( + const float horizontalSpeedCmS, + const float verticalSpeedCmS, + const uint16_t maxAbsAttitudeDeciDeg, + const uint16_t horizontalLimitCmS, + const uint16_t verticalLimitCmS, + const uint16_t attitudeLimitDeciDeg) +{ + const float absVerticalSpeedCmS = verticalSpeedCmS < 0.0f ? -verticalSpeedCmS : verticalSpeedCmS; + + return horizontalSpeedCmS <= horizontalLimitCmS && + absVerticalSpeedCmS <= verticalLimitCmS && + maxAbsAttitudeDeciDeg <= attitudeLimitDeciDeg; +} + +static inline bool vtolMcProtectionUsingVelocityFallback( + const bool horizontalVelocityUsable, + const bool verticalVelocityUsable) +{ + return !horizontalVelocityUsable && verticalVelocityUsable; +} + +static inline uint16_t vtolMcProtectionSettleTimeMs(const bool horizontalVelocityUsable) +{ + return horizontalVelocityUsable ? VTOL_MC_SETTLE_TIME_MS : VTOL_MC_FALLBACK_SETTLE_TIME_MS; +} + +static inline bool vtolMcProtectionSettleConditionsMetWithFallback( + const bool horizontalVelocityUsable, + const bool verticalVelocityUsable, + const float horizontalSpeedCmS, + const float verticalSpeedCmS, + const uint16_t maxAbsAttitudeDeciDeg, + const uint16_t horizontalLimitCmS, + const uint16_t verticalLimitCmS, + const uint16_t attitudeLimitDeciDeg) +{ + if (!verticalVelocityUsable) { + return false; + } + + if (horizontalVelocityUsable) { + return vtolMcProtectionSettleConditionsMet( + horizontalSpeedCmS, + verticalSpeedCmS, + maxAbsAttitudeDeciDeg, + horizontalLimitCmS, + verticalLimitCmS, + attitudeLimitDeciDeg); + } + + const float absVerticalSpeedCmS = verticalSpeedCmS < 0.0f ? -verticalSpeedCmS : verticalSpeedCmS; + return absVerticalSpeedCmS <= verticalLimitCmS && + maxAbsAttitudeDeciDeg <= attitudeLimitDeciDeg; +} + +static inline bool vtolMcProtectionUpdateSettleState( + vtolMcProtectionSettleState_t *state, + const bool conditionsMet, + const uint16_t settleTimeMs, + const timeMs_t nowMs) +{ + if (!conditionsMet) { + state->stableSinceMs = 0; + state->elapsedMs = 0; + return false; + } + + if (settleTimeMs == 0) { + state->stableSinceMs = nowMs; + state->elapsedMs = 0; + return true; + } + + if (state->stableSinceMs == 0) { + state->stableSinceMs = nowMs; + state->elapsedMs = 0; + return false; + } + + const timeMs_t elapsedMs = nowMs - state->stableSinceMs; + state->elapsedMs = elapsedMs > UINT16_MAX ? UINT16_MAX : elapsedMs; + return elapsedMs >= settleTimeMs; +} + +static inline bool vtolMcProtectionShouldRelaxAltitude( + const bool active, + const bool automaticThrottle, + const bool velocityUsable, + const bool holdOrTransitionState, + const bool landingState, + const bool emergencyState, + const bool altitudeStickActive, + const float horizontalSpeedCmS, + const uint16_t horizontalLimitCmS, + const bool captureActive) +{ + return active && + automaticThrottle && + holdOrTransitionState && + !landingState && + !emergencyState && + !altitudeStickActive && + (captureActive || (velocityUsable && horizontalSpeedCmS > horizontalLimitCmS)); +} + +static inline bool vtolMcProtectionLandingBumpAllowed( + const bool isVtolMcMode, + const bool navLandingActive, + const bool aglTrusted, + const float aglCm, + const bool verticalVelocityUsable, + const float verticalSpeedCmS, + const float desiredVerticalSpeedCmS, + const float finalDescentDemandLimitCmS, + const float verticalSpeedLimitCmS) +{ + if (!isVtolMcMode) { + return true; + } + + if (aglTrusted) { + return aglCm <= VTOL_MC_LANDING_BUMP_SAFE_AGL_CM; + } + + if (!navLandingActive || !verticalVelocityUsable) { + return false; + } + + const float absVerticalSpeedCmS = verticalSpeedCmS < 0.0f ? -verticalSpeedCmS : verticalSpeedCmS; + return desiredVerticalSpeedCmS > -finalDescentDemandLimitCmS && + absVerticalSpeedCmS <= verticalSpeedLimitCmS; +} + +static inline int16_t vtolMcProtectionLandingProbeThrottleDrop( + const int16_t idleThrottle, + const int16_t hoverThrottle) +{ + const int16_t hoverRange = hoverThrottle > idleThrottle ? hoverThrottle - idleThrottle : 0; + int16_t drop = (hoverRange * VTOL_MC_LANDING_PROBE_THROTTLE_DROP_PERCENT) / 100; + + if (drop < VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MIN) { + drop = VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MIN; + } else if (drop > VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MAX) { + drop = VTOL_MC_LANDING_PROBE_THROTTLE_DROP_MAX; + } + + return drop < hoverRange ? drop : hoverRange; +} + +static inline int16_t vtolMcProtectionLandingProbeThrottle( + const int16_t startThrottle, + const int16_t idleThrottle, + const int16_t hoverThrottle, + const timeMs_t startedAtMs, + const timeMs_t nowMs) +{ + const int16_t safeStartThrottle = startThrottle > idleThrottle ? startThrottle : idleThrottle; + const int16_t throttleDrop = vtolMcProtectionLandingProbeThrottleDrop(idleThrottle, hoverThrottle); + const int16_t targetThrottle = safeStartThrottle > idleThrottle + throttleDrop ? safeStartThrottle - throttleDrop : idleThrottle; + + if (VTOL_MC_LANDING_PROBE_RAMP_TIME_MS == 0) { + return targetThrottle; + } + + const timeMs_t elapsedMs = nowMs - startedAtMs; + if (elapsedMs >= VTOL_MC_LANDING_PROBE_RAMP_TIME_MS) { + return targetThrottle; + } + + return safeStartThrottle - ((safeStartThrottle - targetThrottle) * (int32_t)elapsedMs) / VTOL_MC_LANDING_PROBE_RAMP_TIME_MS; +} + +static inline bool vtolMcProtectionLandingProbeAirborneResponse( + const bool aglTrusted, + const float startAglCm, + const float currentAglCm, + const bool verticalVelocityUsable, + const float startVerticalSpeedCmS, + const float currentVerticalSpeedCmS, + const float accZG) +{ + if (aglTrusted && currentAglCm < startAglCm - VTOL_MC_LANDING_PROBE_AGL_DROP_CM) { + return true; + } + + if (accZG < VTOL_MC_LANDING_PROBE_LOW_G_THRESHOLD) { + return true; + } + + return verticalVelocityUsable && + (currentVerticalSpeedCmS < startVerticalSpeedCmS - VTOL_MC_LANDING_PROBE_VEL_DELTA_CM_S || + currentVerticalSpeedCmS < -VTOL_MC_LANDING_PROBE_VEL_ABS_CM_S); +} + +static inline float vtolMcProtectionCommandScaleForSpeed(const float horizontalSpeedCmS) +{ + if (horizontalSpeedCmS <= VTOL_MC_COMMAND_SHAPE_START_CM_S) { + return 1.0f; + } + + if (horizontalSpeedCmS >= VTOL_MC_COMMAND_SHAPE_FULL_CM_S) { + return VTOL_MC_COMMAND_SHAPE_MIN_SCALE; + } + + const float progress = (horizontalSpeedCmS - VTOL_MC_COMMAND_SHAPE_START_CM_S) / + (float)(VTOL_MC_COMMAND_SHAPE_FULL_CM_S - VTOL_MC_COMMAND_SHAPE_START_CM_S); + return 1.0f - progress * (1.0f - VTOL_MC_COMMAND_SHAPE_MIN_SCALE); +} + +static inline uint16_t vtolMcProtectionCommandScalePermille(const float commandScale) +{ + return (uint16_t)(commandScale * 1000.0f + 0.5f); +} + +static inline int16_t vtolMcProtectionApplyCommandScale(const int16_t command, const float commandScale) +{ + if (command == 0 || commandScale >= 0.999f) { + return command; + } + + int16_t shapedCommand = (int16_t)(command * commandScale + (command > 0 ? 0.5f : -0.5f)); + if (shapedCommand == 0) { + shapedCommand = command > 0 ? 1 : -1; + } + + return shapedCommand; +} diff --git a/src/main/navigation/navigation_vtol_mission_logic.h b/src/main/navigation/navigation_vtol_mission_logic.h new file mode 100644 index 00000000000..483231e5e98 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mission_logic.h @@ -0,0 +1,222 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +typedef enum { + NAV_MISSION_VTOL_PRECONDITION_READY = 0, + NAV_MISSION_VTOL_PRECONDITION_WAIT, + NAV_MISSION_VTOL_PRECONDITION_REJECT, +} navMissionVtolTransitionPrecondition_e; + +typedef enum { + NAV_MISSION_VTOL_START_VALIDATION_READY = 0, + NAV_MISSION_VTOL_START_VALIDATION_FAIL_ACTION, + NAV_MISSION_VTOL_START_VALIDATION_REJECT, +} navMissionVtolTransitionStartValidation_e; + +static inline navMissionVtolTransitionPrecondition_e navMissionVtolTransitionPreconditionDisposition( + const bool armed, + const bool failsafeActive, + const bool sensorsCalibrating, + const bool positionUsable, + const bool headingUsable, + const bool mixerProfileModeConfigured, + const bool hotSwitchAvailable, + const bool mixerAtActive) +{ + if (!armed || failsafeActive) { + return NAV_MISSION_VTOL_PRECONDITION_REJECT; + } + + if (sensorsCalibrating || !positionUsable || !headingUsable || mixerAtActive) { + return NAV_MISSION_VTOL_PRECONDITION_WAIT; + } + + if (!mixerProfileModeConfigured || !hotSwitchAvailable) { + return NAV_MISSION_VTOL_PRECONDITION_REJECT; + } + + return NAV_MISSION_VTOL_PRECONDITION_READY; +} + +static inline navMissionVtolTransitionStartValidation_e navMissionVtolTransitionStartValidation( + const bool targetPlatformMatchesRequest, + const bool transitionRequestAllowed) +{ + if (!targetPlatformMatchesRequest) { + return NAV_MISSION_VTOL_START_VALIDATION_REJECT; + } + + if (!transitionRequestAllowed) { + return NAV_MISSION_VTOL_START_VALIDATION_FAIL_ACTION; + } + + return NAV_MISSION_VTOL_START_VALIDATION_READY; +} + +static inline bool navMissionTransitionWaypointAcceptedForAdvance( + const bool transitionStartedAtWaypoint, + const bool ordinaryWaypoint, + const uint16_t waypointEnforceAltitudeCm, + const float transitionStartAltitudeCm, + const float waypointAltitudeCm) +{ + return transitionStartedAtWaypoint && + ordinaryWaypoint && + (waypointEnforceAltitudeCm == 0 || + transitionStartAltitudeCm >= waypointAltitudeCm - waypointEnforceAltitudeCm); +} + +static inline bool navMissionTransitionReachedTargetProfile( + const bool mixerAtAborted, + const bool forcedProfileSwitchCompleted) +{ + // A configured FORCE_SWITCH completes the requested profile change after + // an airspeed timeout, so mission handling must distinguish it from a + // real abort that leaves the old profile active. + return !mixerAtAborted || forcedProfileSwitchCompleted; +} + +static inline bool navMissionShouldAdvanceWaypointAfterTransition( + const bool waypointAcceptedForAdvance, + const bool transitionReachedTargetProfile, + const uint16_t waypointEnforceAltitudeCm, + const float currentAltitudeCm, + const float waypointAltitudeCm) +{ + // A transition may add altitude after the waypoint was accepted. Do not + // make the aircraft loiter back down to the old waypoint, but never skip + // a waypoint while below its enforced minimum altitude. + return waypointAcceptedForAdvance && + transitionReachedTargetProfile && + (waypointEnforceAltitudeCm == 0 || + currentAltitudeCm >= waypointAltitudeCm - waypointEnforceAltitudeCm); +} + +static inline bool navMissionTransitionShouldCaptureBeforeWaypointResume( + const bool missionTransitionActive, + const bool transitionSucceeded, + const bool transitionedToMultirotor, + const bool currentStateIsMultirotor, + const bool waypointShouldAdvance) +{ + // A fixed-wing model may travel well beyond the transition waypoint while + // it decelerates. Capture its real MC motion before trying to revisit a + // HOLD_TIME, LAND, or altitude-incomplete waypoint. + return missionTransitionActive && + transitionSucceeded && + transitionedToMultirotor && + currentStateIsMultirotor && + !waypointShouldAdvance; +} + +static inline bool navMissionTransitionShouldRebaseWaypointBeforeResume( + const bool missionTransitionActive, + const bool transitionSucceeded, + const bool waypointShouldAdvance, + const bool missionCapturePending) +{ + // Do not let the bearing that authorized the transition immediately mark + // the same waypoint as reached after the new platform takes control. + return missionTransitionActive && + transitionSucceeded && + !waypointShouldAdvance && + !missionCapturePending; +} + +static inline bool navMissionVtolTransitionSuppressesWaypointRestartGuard( + const bool missionTransitionInProgress) +{ + return missionTransitionInProgress; +} + +static inline bool navMissionVtolTransitionSuppressesWaypointFallbackToRth( + const bool missionTransitionInProgress) +{ + return missionTransitionInProgress; +} + +static inline bool navMissionVtolTransitionHoldsWaypointSelector( + const bool missionTransitionInProgress, + const bool waypointModeSwitchActive) +{ + return missionTransitionInProgress && waypointModeSwitchActive; +} + +static inline bool navMissionShouldHoldMultirotorWaypointForAltitude( + const bool isMultirotor, + const bool waypointAltitudeEnforceActive, + const uint16_t waypointEnforceAltitudeCm, + const float waypointDistanceCm, + const float waypointInitialDistanceCm, + const uint16_t waypointRadiusCm, + const float altitudeErrorCm) +{ + return isMultirotor && + waypointEnforceAltitudeCm > 0 && + (waypointAltitudeEnforceActive || waypointDistanceCm <= waypointRadiusCm || waypointInitialDistanceCm <= waypointRadiusCm) && + altitudeErrorCm >= waypointEnforceAltitudeCm; +} + +static inline bool navMissionUpdateMultirotorWaypointAltitudeEnforceActive( + const bool isMultirotor, + const bool waypointAltitudeEnforceActive, + const bool forceFromWaypointStart, + const uint16_t waypointEnforceAltitudeCm, + const float waypointDistanceCm, + const float waypointInitialDistanceCm, + const uint16_t waypointRadiusCm) +{ + return waypointAltitudeEnforceActive || + (isMultirotor && + waypointEnforceAltitudeCm > 0 && + (forceFromWaypointStart || waypointDistanceCm <= waypointRadiusCm || waypointInitialDistanceCm <= waypointRadiusCm)); +} + +static inline bool navMissionShouldForceFirstGeoWaypointAltitudeFromStart( + const bool isMultirotor, + const bool firstGeoWaypoint) +{ + return isMultirotor && firstGeoWaypoint; +} + +static inline bool navMissionShouldHoldFirstMultirotorWaypointXyForAltitude( + const bool isMultirotor, + const bool waypointAltitudeEnforceFromStart, + const uint16_t waypointEnforceAltitudeCm, + const float altitudeErrorCm) +{ + return isMultirotor && + waypointAltitudeEnforceFromStart && + waypointEnforceAltitudeCm > 0 && + altitudeErrorCm >= waypointEnforceAltitudeCm; +} + +static inline float navMissionMultirotorWaypointAltitudeTarget( + const bool isMultirotor, + const bool waypointAltitudeEnforceActive, + const float waypointAltitudeCm, + const float interpolatedAltitudeCm) +{ + return (isMultirotor && waypointAltitudeEnforceActive) ? + waypointAltitudeCm : + interpolatedAltitudeCm; +} diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 009b2d486f8..84bb6ce41c4 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -690,6 +690,13 @@ static int logicConditionGetWaypointOperandValue(int operand) { } } +#ifdef USE_AUTO_TRANSITION +static int logicConditionGetAutoTransitionTargetStabilizedOperandValue(flight_dynamics_index_t axis) +{ + return getAutoTransitionTargetStabilizedInput(axis); +} +#endif + static int logicConditionGetFlightOperandValue(int operand) { switch (operand) { @@ -898,6 +905,20 @@ static int logicConditionGetFlightOperandValue(int operand) { return axisPID[PITCH]; break; +#ifdef USE_AUTO_TRANSITION + case LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_ROLL: + return logicConditionGetAutoTransitionTargetStabilizedOperandValue(FD_ROLL); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_PITCH: + return logicConditionGetAutoTransitionTargetStabilizedOperandValue(FD_PITCH); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_YAW: + return logicConditionGetAutoTransitionTargetStabilizedOperandValue(FD_YAW); + break; +#endif + case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT32_MAX); break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 517074fe18b..dcdc2e7b402 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -157,6 +157,11 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED, // cm/s // 47 LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION, // deg // 48 LOGIC_CONDITION_OPERAND_FLIGHT_RELATIVE_WIND_OFFSET, // deg // 49 +#ifdef USE_AUTO_TRANSITION + LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_ROLL, // 50 + LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_PITCH, // 51 + LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_YAW, // 52 +#endif } logicFlightOperands_e; typedef enum { diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index dc9d0ec270a..f8527b193f5 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -108,7 +108,7 @@ static pt1Filter_t amperageFilterState; batteryState_e batteryState; const batteryProfile_t *currentBatteryProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 3); +PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 4); void pgResetFn_batteryProfiles(batteryProfile_t *instance) { diff --git a/src/main/target/common.h b/src/main/target/common.h index e4b7b4013fd..f2e6e4951ef 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -426,8 +426,9 @@ #define USE_TELEMETRY_SBUS2 #endif -//Designed to free space of F722 and F411 MCUs +// Keep larger optional features off 512 KB targets to preserve flash space. #if (MCU_FLASH_SIZE > 512) +#define USE_AUTO_TRANSITION #define USE_TELEMETRY_MAVLINK #define USE_SERIALRX_MAVLINK #define USE_TELEMETRY_SIM diff --git a/src/test/cmake/CMakeListsGTest.txt.in b/src/test/cmake/CMakeListsGTest.txt.in index b4dd8c81100..cc23f361329 100644 --- a/src/test/cmake/CMakeListsGTest.txt.in +++ b/src/test/cmake/CMakeListsGTest.txt.in @@ -6,6 +6,7 @@ include(ExternalProject) ExternalProject_Add(googletest GIT_REPOSITORY https://github.com/google/googletest.git GIT_TAG main + UPDATE_DISCONNECTED 1 SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src" BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build" CONFIGURE_COMMAND "" diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index a48c140ba54..748b1084559 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -72,6 +72,11 @@ set_property(SOURCE gps_null_port_unittest.cc PROPERTY definitions GPS_NULL_PORT set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c") set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER) +set_property(SOURCE mixer_transition_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE mixer_transition_policy_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE mixer_transition_scenarios_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE navigation_vtol_mission_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE vtol_mc_protection_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) set_property(SOURCE mavlink_unittest.cc PROPERTY depends "fc/fc_mavlink.c" "mavlink/mavlink_command.c" "mavlink/mavlink_guided.c" "mavlink/mavlink_mission.c" "mavlink/mavlink_modes.c" "mavlink/mavlink_ports.c" diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc new file mode 100644 index 00000000000..4368658d739 --- /dev/null +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -0,0 +1,922 @@ +#include + +extern "C" { +#include "flight/mixer_transition_logic.h" +} + +TEST(MixerTransitionLogicTest, LegacyManualSessionStaysLegacyAfterHotSwitch) +{ + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + true, + false, + false, + false); + + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_LEGACY, sessionMode); + EXPECT_FALSE(mixerTransitionManualControllerEnabled(true, sessionMode)); + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + false, + true, + true, + false); + + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, sessionMode); +} + +TEST(MixerTransitionLogicTest, AutoManualSessionStaysAutoAcrossProfileChanges) +{ + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + true, + false, + true, + false); + + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, sessionMode); + EXPECT_TRUE(mixerTransitionManualControllerEnabled(false, sessionMode)); +} + +TEST(MixerTransitionLogicTest, TailsitterPairRequiresTheOrientationOffsetOnlyInMcProfile) +{ + EXPECT_TRUE(mixerTransitionIsTailSitterProfilePair( + false, true, true, + true, false, false)); + + EXPECT_TRUE(mixerTransitionIsTailSitterProfilePair( + true, false, false, + false, true, true)); + + EXPECT_FALSE(mixerTransitionIsTailSitterProfilePair( + false, true, false, + true, false, false)); + + EXPECT_FALSE(mixerTransitionIsTailSitterProfilePair( + true, false, true, + false, true, true)); +} + +TEST(MixerTransitionLogicTest, TailsitterFirstVersionAllowsOnlyManualNonDynamicRequests) +{ + EXPECT_TRUE(mixerTransitionTailSitterRequestIsSupported( + MIXERAT_REQUEST_MANUAL_TO_FW, true, false, false)); + EXPECT_TRUE(mixerTransitionTailSitterRequestIsSupported( + MIXERAT_REQUEST_MANUAL_TO_MC, true, false, false)); + + EXPECT_FALSE(mixerTransitionTailSitterRequestIsSupported( + MIXERAT_REQUEST_MISSION_TO_FW, true, false, false)); + EXPECT_FALSE(mixerTransitionTailSitterRequestIsSupported( + MIXERAT_REQUEST_RTH, true, false, false)); + EXPECT_FALSE(mixerTransitionTailSitterRequestIsSupported( + MIXERAT_REQUEST_MANUAL_TO_FW, true, true, false)); + EXPECT_FALSE(mixerTransitionTailSitterRequestIsSupported( + MIXERAT_REQUEST_MANUAL_TO_MC, true, false, true)); + + // Non-tailsitter profile pairs preserve their existing request policy. + EXPECT_TRUE(mixerTransitionTailSitterRequestIsSupported( + MIXERAT_REQUEST_MISSION_TO_FW, false, true, true)); +} + +TEST(MixerTransitionLogicTest, TailsitterMcCaptureUsesTheExistingFortyFiveDegreeTarget) +{ + EXPECT_FALSE(mixerTransitionTailSitterCapturePitchReached(349)); + EXPECT_TRUE(mixerTransitionTailSitterCapturePitchReached(350)); + EXPECT_TRUE(mixerTransitionTailSitterCapturePitchReached(450)); + EXPECT_TRUE(mixerTransitionTailSitterCapturePitchReached(550)); + EXPECT_FALSE(mixerTransitionTailSitterCapturePitchReached(551)); + + EXPECT_TRUE(mixerTransitionTailSitterNeedsMcCapture(MIXERAT_REQUEST_MANUAL_TO_MC, true)); + EXPECT_FALSE(mixerTransitionTailSitterNeedsMcCapture(MIXERAT_REQUEST_MANUAL_TO_FW, true)); + EXPECT_FALSE(mixerTransitionTailSitterNeedsMcCapture(MIXERAT_REQUEST_MANUAL_TO_MC, false)); +} + +TEST(MixerTransitionLogicTest, TailsitterMcCaptureReversesThroughTheMcToFwPath) +{ + EXPECT_TRUE(mixerTransitionTailSitterCaptureShouldReverse( + true, true, false, true, 1, 0)); + + EXPECT_FALSE(mixerTransitionTailSitterCaptureShouldReverse( + true, true, true, true, 1, 0)); + EXPECT_FALSE(mixerTransitionTailSitterCaptureShouldReverse( + true, true, false, false, 1, 0)); + EXPECT_FALSE(mixerTransitionTailSitterCaptureShouldReverse( + true, true, false, true, 1, 1)); + EXPECT_FALSE(mixerTransitionTailSitterCaptureShouldReverse( + false, true, false, true, 1, 0)); +} + +TEST(MixerTransitionLogicTest, CompletedAutoSessionReleasesOnFallingEdgeToEndpoint) +{ + EXPECT_FALSE(mixerTransitionKeepCompletedAutoSession( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + true, + 0, + 1)); + + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, mixerTransitionUpdateManualSessionMode( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + true, + true, + false)); +} + +TEST(MixerTransitionLogicTest, CompletedAutoSessionReleasesWhenSwitchMatchesActiveProfile) +{ + EXPECT_FALSE(mixerTransitionKeepCompletedAutoSession( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + true, + 0, + 0)); + + EXPECT_FALSE(mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + 0, + 0)); + + EXPECT_FALSE(mixerTransitionKeepCompletedAutoSession( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + false, + 0, + 1)); +} + +TEST(MixerTransitionLogicTest, CompletedAutoSessionEndpointConfirmationIsDetected) +{ + EXPECT_TRUE(mixerTransitionCompletedAutoSessionEndpointConfirmed( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + false, + 0, + 0)); + + EXPECT_FALSE(mixerTransitionCompletedAutoSessionEndpointConfirmed( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + true, + 0, + 0)); + + EXPECT_FALSE(mixerTransitionCompletedAutoSessionEndpointConfirmed( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + false, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionCompletedAutoSessionEndpointConfirmed( + MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + true, + false, + 0, + 0)); +} + +TEST(MixerTransitionLogicTest, DirectSwitchEndpointReleasesAnyStaleServoHandoff) +{ + EXPECT_TRUE(mixerTransitionDirectSwitchEndpointOwnsServoOutput( + MIXER_TRANSITION_MANUAL_SESSION_NONE, + false, + false, + false, + false, + false, + 1, + 1)); + + EXPECT_FALSE(mixerTransitionDirectSwitchEndpointOwnsServoOutput( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + false, + false, + false, + false, + 1, + 1)); + EXPECT_FALSE(mixerTransitionDirectSwitchEndpointOwnsServoOutput( + MIXER_TRANSITION_MANUAL_SESSION_NONE, + true, + false, + false, + false, + false, + 1, + 1)); + EXPECT_FALSE(mixerTransitionDirectSwitchEndpointOwnsServoOutput( + MIXER_TRANSITION_MANUAL_SESSION_NONE, + false, + true, + false, + false, + false, + 1, + 1)); + EXPECT_FALSE(mixerTransitionDirectSwitchEndpointOwnsServoOutput( + MIXER_TRANSITION_MANUAL_SESSION_NONE, + false, + false, + true, + false, + false, + 1, + 1)); + EXPECT_FALSE(mixerTransitionDirectSwitchEndpointOwnsServoOutput( + MIXER_TRANSITION_MANUAL_SESSION_NONE, + false, + false, + false, + true, + false, + 1, + 1)); + EXPECT_TRUE(mixerTransitionDirectSwitchEndpointOwnsServoOutput( + MIXER_TRANSITION_MANUAL_SESSION_NONE, + false, + false, + false, + true, + true, + 1, + 1)); + EXPECT_FALSE(mixerTransitionDirectSwitchEndpointOwnsServoOutput( + MIXER_TRANSITION_MANUAL_SESSION_NONE, + false, + false, + false, + false, + false, + 0, + 1)); +} + +TEST(MixerTransitionLogicTest, EndpointConfirmationMustBeCheckedBeforeFallingEdgeClearsSession) +{ + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_AUTO; + const bool shouldClearHandoff = mixerTransitionCompletedAutoSessionEndpointConfirmed( + sessionMode, + true, + false, + 0, + 0); + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + false, + true, + true, + false); + + EXPECT_TRUE(shouldClearHandoff); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, sessionMode); +} + +TEST(MixerTransitionLogicTest, CompletedAutoSessionClearsStaleMixingWhileSwitchRemainsInTransition) +{ + EXPECT_TRUE(mixerTransitionShouldClearCompletedAutoMixingRequest( + true, + false, + false, + true)); + + EXPECT_FALSE(mixerTransitionShouldClearCompletedAutoMixingRequest( + true, + false, + true, + true)); + + EXPECT_FALSE(mixerTransitionShouldClearCompletedAutoMixingRequest( + false, + false, + false, + true)); + + EXPECT_FALSE(mixerTransitionShouldClearCompletedAutoMixingRequest( + true, + true, + false, + true)); +} + +TEST(MixerTransitionLogicTest, ManualSwitchReminderShowsAfterCompletedAutoSwitchUntilEndpointConfirmed) +{ + EXPECT_EQ(MIXERAT_DIRECTION_TO_MC, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + true, + true, + 1, + 1, + true)); + + EXPECT_EQ(MIXERAT_DIRECTION_TO_FW, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + true, + false, + 0, + 1, + false)); +} + +TEST(MixerTransitionLogicTest, ManualSwitchReminderClearsWhenEndpointMatches) +{ + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + true, + false, + 1, + 1, + true)); + + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + true, + false, + 0, + 0, + false)); +} + +TEST(MixerTransitionLogicTest, ManualSwitchReminderDoesNotShowForLegacyOrActiveTransition) +{ + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + false, + true, + true, + 1, + 1, + true)); + + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + true, + true, + true, + 1, + 1, + true)); + + EXPECT_EQ(MIXERAT_DIRECTION_NONE, mixerTransitionManualSwitchReminderDirection( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + false, + true, + 1, + 1, + true)); +} + +TEST(MixerTransitionLogicTest, NavigationHandbackHoldsProfileWhenManualSwitchDiffers) +{ + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + false, + true, + false, + false, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldHoldProfile( + false, + false, + true, + false, + false, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + true, + true, + false, + false, + 0, + 1)); +} + +TEST(MixerTransitionLogicTest, NavigationHandbackHoldsProfileWhenSwitchStillInTransitionPosition) +{ + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + false, + true, + false, + true, + 1, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldClear( + false, + true, + 1, + 1)); + + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + false, + false, + 1, + 1)); +} + +TEST(MixerTransitionLogicTest, NavigationHandbackClearsForNewNavigationOrMatchingEndpoint) +{ + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + true, + false, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldClear( + false, + true, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + false, + true, + true, + false, + 0, + 1)); +} + +TEST(MixerTransitionLogicTest, NavigationOwnershipIgnoresManualTransitionInput) +{ + EXPECT_FALSE(mixerTransitionManualInputAllowed(true, false)); + EXPECT_FALSE(mixerTransitionManualInputAllowed(false, true)); + EXPECT_FALSE(mixerTransitionManualMixingRequestMayUpdate(false, false, true, false)); + EXPECT_FALSE(mixerTransitionManualMixingRequestMayUpdate(false, false, false, true)); + EXPECT_TRUE(mixerTransitionManualMixingRequestMayUpdate(false, false, false, false)); + + // Entering navigation clears a previous manual session. A physical switch + // movement during the mission cannot create another manual session. + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, mixerTransitionUpdateManualSessionMode( + MIXER_TRANSITION_MANUAL_SESSION_AUTO, + false, + false, + true, + true)); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, mixerTransitionUpdateManualSessionMode( + MIXER_TRANSITION_MANUAL_SESSION_NONE, + true, + false, + true, + true)); +} + +TEST(MixerTransitionLogicTest, NavigationHandbackIgnoresIntermediatePositionUntilEndpointMatches) +{ + // A mission ended in FW/profile 0 while the pilot switch remains at MC/profile 1. + // Passing the three-position switch through transition must not create a new FW->MC request. + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + false, + true, + false, + true, + 0, + 1)); + EXPECT_FALSE(mixerTransitionManualInputAllowed(false, true)); + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldClear( + false, + true, + 0, + 1)); + + // The handback ends only after the FW endpoint is reached. + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + false, + false, + 0, + 0)); + + // The reciprocal MC handback follows exactly the same rule. + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + false, + true, + false, + true, + 1, + 0)); + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldClear( + false, + true, + 1, + 0)); + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + false, + false, + 1, + 1)); +} + +TEST(MixerTransitionLogicTest, ActiveModesReportEffectiveVtolStateInsteadOfRawSwitchState) +{ + EXPECT_FALSE(mixerTransitionProfile2ShouldReportActive(0)); + EXPECT_TRUE(mixerTransitionProfile2ShouldReportActive(1)); + + // A transition switch in the middle position is not an active transition + // when navigation ignores it. Both real transition phases remain visible. + EXPECT_FALSE(mixerTransitionModeShouldReportActive(false, false)); + EXPECT_TRUE(mixerTransitionModeShouldReportActive(false, true)); + EXPECT_TRUE(mixerTransitionModeShouldReportActive(true, false)); +} + +TEST(MixerTransitionLogicTest, ProfileSwitchAutotransitionStartsOnlyWhenArmedAndClear) +{ + EXPECT_TRUE(mixerTransitionProfileSwitchShouldStartAutoTransition( + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + 1, + 0)); + + EXPECT_FALSE(mixerTransitionProfileSwitchShouldStartAutoTransition( + true, + false, + true, + true, + true, + false, + false, + false, + false, + false, + 1, + 0)); + + EXPECT_FALSE(mixerTransitionProfileSwitchShouldStartAutoTransition( + false, + true, + true, + true, + true, + false, + false, + false, + false, + false, + 1, + 0)); +} + +TEST(MixerTransitionLogicTest, ProfileSwitchAutotransitionKeepsDisarmedDirectSwitchForPreflight) +{ + EXPECT_FALSE(mixerTransitionProfileSwitchDirectSwitchAllowed( + true, + true, + true)); + + EXPECT_TRUE(mixerTransitionProfileSwitchDirectSwitchAllowed( + true, + false, + true)); + + EXPECT_TRUE(mixerTransitionProfileSwitchDirectSwitchAllowed( + false, + true, + true)); + + EXPECT_TRUE(mixerTransitionProfileSwitchDirectSwitchAllowed( + true, + true, + false)); +} + +TEST(MixerTransitionLogicTest, ProfileSwitchAutotransitionDoesNotFightOwnedStates) +{ + EXPECT_FALSE(mixerTransitionProfileSwitchShouldStartAutoTransition( + true, + true, + true, + true, + true, + false, + false, + false, + true, + false, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionProfileSwitchShouldStartAutoTransition( + true, + true, + true, + true, + true, + false, + true, + false, + false, + false, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionProfileSwitchShouldStartAutoTransition( + true, + true, + true, + true, + true, + false, + false, + true, + false, + false, + 0, + 1)); +} + +TEST(MixerTransitionLogicTest, ProfileSwitchAutotransitionAbortsWhenSwitchReturnsToCurrentProfile) +{ + EXPECT_TRUE(mixerTransitionProfileSwitchShouldAbortToCurrentProfile( + true, + true, + false, + 1, + 1)); + + EXPECT_FALSE(mixerTransitionProfileSwitchShouldAbortToCurrentProfile( + true, + true, + true, + 1, + 1)); + + EXPECT_FALSE(mixerTransitionProfileSwitchShouldAbortToCurrentProfile( + true, + true, + false, + 1, + 0)); +} + +TEST(MixerTransitionLogicTest, LegacySessionIgnoresAutoControllerAfterProfileHotSwitch) +{ + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + true, + false, + false, + false); + + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_LEGACY, sessionMode); + EXPECT_FALSE(mixerTransitionManualControllerEnabled(true, sessionMode)); + EXPECT_EQ(500, mixerTransitionUpdateServoInput( + 0, + sessionMode == MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + true, + false, + false, + true, + 0.18f)); +} + +TEST(MixerTransitionLogicTest, LegacyServoInputMatchesPrePrFixedEndpoint) +{ + EXPECT_EQ(500, mixerTransitionUpdateServoInput( + 0, + true, + true, + false, + false, + true, + 0.12f)); + + EXPECT_EQ(0, mixerTransitionUpdateServoInput( + 500, + true, + false, + false, + false, + true, + 0.12f)); +} + +TEST(MixerTransitionLogicTest, AutoServoInputDoesNotMoveBackwardsDuringMcToFw) +{ + int16_t servoInput = 0; + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + true, + true, + false, + true, + 0.30f); + EXPECT_EQ(150, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + true, + true, + false, + true, + 0.62f); + EXPECT_EQ(310, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + true, + true, + false, + true, + 0.58f); + EXPECT_EQ(310, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + false, + true, + false, + true, + 0.20f); + EXPECT_EQ(310, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + false, + true, + true, + true, + 0.20f); + EXPECT_EQ(500, servoInput); + + servoInput = mixerTransitionUpdateServoInput( + servoInput, + false, + false, + false, + false, + false, + 0.0f); + EXPECT_EQ(0, servoInput); +} + +TEST(MixerTransitionLogicTest, AutoServoBlendUsesScaleRampTimerInsteadOfAirspeedProgress) +{ + const float blendAtTransitionStart = mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, + true, + MIXERAT_DIRECTION_TO_FW, + 16000, + 10); + + EXPECT_LT(blendAtTransitionStart, 0.01f); + EXPECT_EQ(0, mixerTransitionUpdateServoInput( + 0, + false, + true, + true, + false, + true, + blendAtTransitionStart)); + + EXPECT_FLOAT_EQ(0.65f, mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, + true, + MIXERAT_DIRECTION_TO_FW, + 1000, + 650)); +} + +TEST(MixerTransitionLogicTest, AutoServoBlendCountsBackDownDuringFwToMc) +{ + EXPECT_FLOAT_EQ(0.75f, mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, + true, + MIXERAT_DIRECTION_TO_MC, + 1000, + 250)); + + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeServoBlendToFw( + false, + false, + true, + true, + true, + MIXERAT_DIRECTION_TO_FW, + 0, + 0)); +} + +TEST(MixerTransitionLogicTest, AutoServoBlendStaysLegacyStaticWhenDynamicMixerIsDisabled) +{ + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, + false, + MIXERAT_DIRECTION_TO_FW, + 1000, + 0)); + + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, + false, + MIXERAT_DIRECTION_TO_FW, + 1000, + 100)); + + EXPECT_EQ(500, mixerTransitionUpdateServoInput( + 0, + false, + true, + true, + false, + true, + 1.0f)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffUsesFullScaleRampTimeAfterHotSwitchWhenDynamicMixerIsEnabled) +{ + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(true, 1000, 0)); + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(true, 1000, 250)); + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(true, 1000, 1200)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffUsesConfiguredScaleRampWhenDynamicMixerIsDisabled) +{ + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(false, 1000, 0)); + EXPECT_EQ(1000, mixerTransitionComputeServoHandoffDurationMs(false, 1000, 750)); + EXPECT_EQ(0, mixerTransitionComputeServoHandoffDurationMs(false, 0, 750)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffExpiresAtConfiguredDuration) +{ + EXPECT_FALSE(mixerTransitionServoHandoffExpired(0, 1000)); + EXPECT_FALSE(mixerTransitionServoHandoffExpired(999, 1000)); + EXPECT_TRUE(mixerTransitionServoHandoffExpired(1000, 1000)); + EXPECT_TRUE(mixerTransitionServoHandoffExpired(1500, 1000)); + EXPECT_TRUE(mixerTransitionServoHandoffExpired(0, 0)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffProgressIsBounded) +{ + EXPECT_FLOAT_EQ(0.0f, mixerTransitionServoHandoffProgress(0, 1000)); + EXPECT_FLOAT_EQ(0.5f, mixerTransitionServoHandoffProgress(500, 1000)); + EXPECT_FLOAT_EQ(1.0f, mixerTransitionServoHandoffProgress(1000, 1000)); + EXPECT_FLOAT_EQ(1.0f, mixerTransitionServoHandoffProgress(1500, 1000)); + EXPECT_FLOAT_EQ(1.0f, mixerTransitionServoHandoffProgress(0, 0)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffBlendStartsFromCapturedOutputAfterHotSwitch) +{ + EXPECT_EQ(1366, mixerTransitionBlendCapturedServoOutput(1366, 980, 0.0f)); + EXPECT_EQ(1270, mixerTransitionBlendCapturedServoOutput(1366, 980, 0.25f)); + EXPECT_EQ(1173, mixerTransitionBlendCapturedServoOutput(1366, 980, 0.50f)); + EXPECT_EQ(980, mixerTransitionBlendCapturedServoOutput(1366, 980, 1.0f)); +} + +TEST(MixerTransitionLogicTest, ServoHandoffBlendReturnsSmoothlyAfterAbort) +{ + EXPECT_EQ(1651, mixerTransitionBlendCapturedServoOutput(1651, 1224, 0.0f)); + EXPECT_EQ(1438, mixerTransitionBlendCapturedServoOutput(1651, 1224, 0.50f)); + EXPECT_EQ(1224, mixerTransitionBlendCapturedServoOutput(1651, 1224, 1.0f)); +} diff --git a/src/test/unit/mixer_transition_policy_unittest.cc b/src/test/unit/mixer_transition_policy_unittest.cc new file mode 100644 index 00000000000..b6845e828a5 --- /dev/null +++ b/src/test/unit/mixer_transition_policy_unittest.cc @@ -0,0 +1,1087 @@ +#include + +extern "C" { +#include "flight/mixer_transition_logic.h" +} + +TEST(MixerTransitionPolicyTest, RequestGatingWorksWhenFixedWingProfileIsIndexZero) +{ + const flyingPlatformType_e profileTypes[] = { + PLATFORM_AIRPLANE, + PLATFORM_TRICOPTER, + }; + const int targetFwIndex = 0; + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_FW, + false, + true, + true, + false, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_RTH, + false, + true, + true, + false, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_RTH, + false, + true, + true, + true, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); +} + +TEST(MixerTransitionPolicyTest, RequestGatingWorksWhenFixedWingProfileIsIndexOne) +{ + const flyingPlatformType_e profileTypes[] = { + PLATFORM_TRICOPTER, + PLATFORM_AIRPLANE, + }; + const int targetFwIndex = 1; + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MISSION_TO_FW, + false, + true, + true, + false, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MISSION_TO_FW, + false, + true, + false, + false, + profileTypes[targetFwIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetFwIndex]))); +} + +TEST(MixerTransitionPolicyTest, LandRequestNeedsAutomatedSwitchWhenMultirotorProfileIsIndexZero) +{ + const flyingPlatformType_e profileTypes[] = { + PLATFORM_TRICOPTER, + PLATFORM_AIRPLANE, + }; + const int targetMcIndex = 0; + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_LAND, + true, + false, + true, + false, + profileTypes[targetMcIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetMcIndex]))); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_LAND, + true, + false, + true, + true, + profileTypes[targetMcIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetMcIndex]))); +} + +TEST(MixerTransitionPolicyTest, LandRequestNeedsAutomatedSwitchWhenMultirotorProfileIsIndexOne) +{ + const flyingPlatformType_e profileTypes[] = { + PLATFORM_AIRPLANE, + PLATFORM_TRICOPTER, + }; + const int targetMcIndex = 1; + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_LAND, + true, + false, + true, + false, + profileTypes[targetMcIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetMcIndex]))); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_LAND, + true, + false, + true, + true, + profileTypes[targetMcIndex] == PLATFORM_AIRPLANE, + isMultirotorTypePlatform(profileTypes[targetMcIndex]))); +} + +TEST(MixerTransitionPolicyTest, NavigationFwToMcProtectionNeedsAutomatedSwitch) +{ + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_FW_TO_MC_PROTECTION, + true, + false, + true, + false, + false, + true)); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_FW_TO_MC_PROTECTION, + true, + false, + true, + true, + false, + true)); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_FW_TO_MC_PROTECTION, + true, + false, + true, + true, + true, + false)); +} + +TEST(MixerTransitionPolicyTest, ManualRequestsNeedMixerProfileModeAndMatchingTargetType) +{ + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_FW, + false, + true, + false, + false, + true, + false)); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_FW, + false, + true, + true, + false, + false, + false)); + + EXPECT_FALSE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_MC, + true, + false, + true, + false, + true, + false)); + + EXPECT_TRUE(mixerTransitionIsRequestAllowed( + MIXERAT_REQUEST_MANUAL_TO_MC, + true, + false, + true, + false, + false, + true)); +} + +TEST(MixerTransitionPolicyTest, OnlyNavigationOwnedRequestsMayContinueDuringFailsafe) +{ + EXPECT_TRUE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_RTH)); + EXPECT_TRUE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_LAND)); + + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_NONE)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MISSION_TO_FW)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MISSION_TO_MC)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MANUAL_TO_FW)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_MANUAL_TO_MC)); + EXPECT_TRUE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_FW_TO_MC_PROTECTION)); + EXPECT_FALSE(mixerTransitionRequestAllowedDuringFailsafe(MIXERAT_REQUEST_ABORT)); +} + +TEST(MixerTransitionPolicyTest, FailsafeKeepsPostSwitchOutputRampAfterRequestIsCleared) +{ + EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_RTH, false, false)); + EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_LAND, false, false)); + EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_FW_TO_MC_PROTECTION, false, false)); + + EXPECT_TRUE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, false, false)); + EXPECT_TRUE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, true, false)); + EXPECT_TRUE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, false, true)); + + EXPECT_FALSE(mixerTransitionShouldAbortForFailsafe(MIXERAT_REQUEST_NONE, true, true)); +} + +TEST(MixerTransitionPolicyTest, FwToMcProtectionAirspeedTriggerNeedsTrustedLowAirspeed) +{ + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(false, true, 700, true, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, false, 700, true, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, true, 0, true, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, true, 700, false, 650.0f)); + EXPECT_FALSE(mixerTransitionFwToMcProtectionTriggered(true, true, 700, true, 701.0f)); + + EXPECT_TRUE(mixerTransitionFwToMcProtectionTriggered(true, true, 700, true, 700.0f)); + EXPECT_TRUE(mixerTransitionFwToMcProtectionTriggered(true, true, 700, true, 650.0f)); +} + +TEST(MixerTransitionPolicyTest, NavigationOwnsProfileSwitchOnlyForArmedVtolAutoStates) +{ + EXPECT_FALSE(mixerTransitionNavigationOwnsProfileSwitch( + false, + true, + true, + false, + false, + false)); + + EXPECT_FALSE(mixerTransitionNavigationOwnsProfileSwitch( + true, + false, + true, + false, + false, + false)); + + EXPECT_FALSE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + false, + false, + false, + false)); + + EXPECT_TRUE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + true, + false, + false, + false)); + + EXPECT_TRUE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + false, + true, + false, + false)); + + EXPECT_TRUE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + false, + false, + true, + false)); + + EXPECT_TRUE(mixerTransitionNavigationOwnsProfileSwitch( + true, + true, + false, + false, + false, + true)); +} + +TEST(MixerTransitionPolicyTest, DynamicScalingDisabledKeepsAllScalesAtFullValues) +{ + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + false, + MIXERAT_DIRECTION_TO_FW, + 0.20f, + 0.30f, + 0.40f, + 0.50f, + 0.25f); + + EXPECT_FLOAT_EQ(1.0f, scales.pusherScale); + EXPECT_FLOAT_EQ(1.0f, scales.liftScale); + EXPECT_FLOAT_EQ(1.0f, scales.mcAuthorityScale); + EXPECT_FLOAT_EQ(1.0f, scales.fwAuthorityScale); + EXPECT_FLOAT_EQ(1.0f, scales.blendToFw); +} + +TEST(MixerTransitionPolicyTest, McToFwDynamicScalingUsesHandoffAndRampProgress) +{ + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + true, + MIXERAT_DIRECTION_TO_FW, + 0.20f, + 0.30f, + 0.40f, + 0.50f, + 0.25f); + + EXPECT_FLOAT_EQ(0.25f, scales.pusherScale); + EXPECT_FLOAT_EQ(0.60f, scales.liftScale); + EXPECT_FLOAT_EQ(0.65f, scales.mcAuthorityScale); + EXPECT_FLOAT_EQ(0.70f, scales.fwAuthorityScale); + EXPECT_FLOAT_EQ(0.70f, scales.blendToFw); +} + +TEST(MixerTransitionPolicyTest, FwToMcDynamicScalingUsesTimerRampForLiftAndPusher) +{ + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + true, + MIXERAT_DIRECTION_TO_MC, + 0.20f, + 0.30f, + 0.40f, + 0.50f, + 0.25f); + + EXPECT_FLOAT_EQ(0.75f, scales.pusherScale); + EXPECT_FLOAT_EQ(0.40f, scales.liftScale); + EXPECT_FLOAT_EQ(0.475f, scales.mcAuthorityScale); + EXPECT_FLOAT_EQ(0.70f, scales.fwAuthorityScale); + EXPECT_FLOAT_EQ(0.70f, scales.blendToFw); +} + +TEST(MixerTransitionPolicyTest, AirspeedProgressRejectsAnIsolatedHighSpike) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.50f; + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.50f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.95f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.50f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.55f, 300, 300, 100, &filter); + + EXPECT_FLOAT_EQ(0.50f, accepted); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.55f, 400, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.55f, 500, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.55f, 600, 300, 100, &filter); + + EXPECT_FLOAT_EQ(0.55f, accepted); +} + +TEST(MixerTransitionPolicyTest, AirspeedProgressRejectsAnIsolatedLowSpike) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.80f; + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.80f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.25f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.80f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.75f, 300, 300, 100, &filter); + + EXPECT_FLOAT_EQ(0.80f, accepted); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.75f, 400, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.75f, 500, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.75f, 600, 300, 100, &filter); + + EXPECT_FLOAT_EQ(0.75f, accepted); +} + +TEST(MixerTransitionPolicyTest, AirspeedProgressUsesTheConservativeEdgeOfASustainedRise) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 8.0f / 13.0f; + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 11.0f / 13.0f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 12.0f / 13.0f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 1.0f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 1.0f, 300, 300, 100, &filter); + + // The first confirmed sample establishes the slew start. It must not + // apply the full 300 ms progress delta in one motor update. + EXPECT_FLOAT_EQ(8.0f / 13.0f, accepted); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 1.0f, 400, 300, 100, &filter); + EXPECT_NEAR(12.0f / 13.0f, accepted, 0.0001f); +} + +TEST(MixerTransitionPolicyTest, AirspeedProgressUsesTheConservativeEdgeOfASustainedFall) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.80f; + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.75f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.60f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.50f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.45f, 300, 300, 100, &filter); + + EXPECT_FLOAT_EQ(0.80f, accepted); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.45f, 400, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.60f, accepted); +} + +TEST(MixerTransitionPolicyTest, ConfirmedMcToFwAirspeedLossRestoresScalesTogether) +{ + const mixerTransitionScaleState_t beforeLoss = mixerTransitionComputeScales( + true, + MIXERAT_DIRECTION_TO_FW, + 0.20f, + 0.30f, + 0.40f, + 0.80f, + 0.65f); + const mixerTransitionScaleState_t afterLoss = mixerTransitionComputeScales( + true, + MIXERAT_DIRECTION_TO_FW, + 0.20f, + 0.30f, + 0.40f, + 0.60f, + 0.65f); + + EXPECT_FLOAT_EQ(beforeLoss.pusherScale, afterLoss.pusherScale); + EXPECT_GT(afterLoss.liftScale, beforeLoss.liftScale); + EXPECT_GT(afterLoss.mcAuthorityScale, beforeLoss.mcAuthorityScale); + EXPECT_LT(afterLoss.fwAuthorityScale, beforeLoss.fwAuthorityScale); +} + +TEST(MixerTransitionPolicyTest, TimerFallbackIsMonotonicAndResetsAirspeedHistory) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.40f; + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.90f, 0, 300, 100, &filter); + EXPECT_TRUE(filter.active); + + accepted = mixerTransitionResolveHandoffProgress(true, false, accepted, 0.55f, 100, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.55f, accepted); + EXPECT_FALSE(filter.active); + + accepted = mixerTransitionResolveHandoffProgress(true, false, accepted, 0.45f, 200, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.55f, accepted); +} + +TEST(MixerTransitionPolicyTest, TimerFallbackSlewsAfterConfirmedAirspeedProgress) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.0f; + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.50f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.50f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.50f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.50f, 300, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.50f, 350, 300, 100, &filter); + EXPECT_NEAR(1.0f / 6.0f, accepted, 0.0001f); + + accepted = mixerTransitionResolveHandoffProgress(true, false, accepted, 0.90f, 360, 300, 100, &filter); + EXPECT_NEAR(0.20f, accepted, 0.0001f); + EXPECT_FALSE(filter.active); + + accepted = mixerTransitionResolveHandoffProgress(true, false, accepted, 0.90f, 460, 300, 100, &filter); + EXPECT_NEAR(0.20f + 1.0f / 3.0f, accepted, 0.0001f); +} + +TEST(MixerTransitionPolicyTest, ReturningAirspeedNeedsANewConfirmationWindow) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = mixerTransitionResolveHandoffProgress(true, false, 0.60f, 0.60f, 0, 300, 100, &filter); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.30f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.30f, 300, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.60f, accepted); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.30f, 400, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.60f, accepted); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.30f, 500, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.30f, accepted); +} + +TEST(MixerTransitionPolicyTest, InterruptedAirspeedSamplesRestartConfirmation) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.40f; + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.80f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.80f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.80f, 500, 300, 100, &filter); + + EXPECT_FLOAT_EQ(0.40f, accepted); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.80f, 700, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.40f, accepted); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.80f, 800, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.40f, accepted); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.80f, 900, 300, 100, &filter); + EXPECT_NEAR(0.40f + 1.0f / 3.0f, accepted, 0.0001f); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.80f, 1000, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.80f, accepted); +} + +TEST(MixerTransitionPolicyTest, DynamicScalingOffAndTimerOnlyKeepExistingBehavior) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + + EXPECT_FLOAT_EQ(1.0f, mixerTransitionResolveHandoffProgress( + false, true, 0.25f, 0.10f, 0, 300, 100, &filter)); + EXPECT_TRUE(filter.active); + EXPECT_FALSE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); + + EXPECT_FLOAT_EQ(0.65f, mixerTransitionResolveHandoffProgress( + true, false, 0.50f, 0.65f, 100, 300, 100, &filter)); + EXPECT_FALSE(filter.active); +} + +TEST(MixerTransitionPolicyTest, DynamicScalingOffStillRejectsAnEndpointSpike) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.0f; + + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 0.80f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.00f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 0.80f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.00f, 300, 300, 100, &filter); + + EXPECT_FLOAT_EQ(1.0f, accepted); + EXPECT_FALSE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); +} + +TEST(MixerTransitionPolicyTest, AirspeedEndpointConfirmationRestartsAfterTimerFallback) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.0f; + + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, false, accepted, 0.2f, 200, 300, 100, &filter); + EXPECT_FALSE(filter.active); + + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 300, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 400, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 500, 300, 100, &filter); + EXPECT_FALSE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); + + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 600, 300, 100, &filter); + EXPECT_TRUE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); +} + +TEST(MixerTransitionPolicyTest, AirspeedConfirmationHandlesMillisWraparound) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.0f; + const timeMs_t startTime = UINT32_MAX - 150U; + + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, startTime, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, startTime + 100U, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, startTime + 200U, 300, 100, &filter); + EXPECT_FALSE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); + + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, startTime + 300U, 300, 100, &filter); + EXPECT_TRUE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); +} + +TEST(MixerTransitionPolicyTest, RecentAirspeedSampleSuppressesOnlyBriefSourceDropouts) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = mixerTransitionResolveHandoffProgress( + false, true, 0.0f, 0.5f, 100, 300, 100, &filter); + + EXPECT_FLOAT_EQ(1.0f, accepted); + EXPECT_TRUE(mixerTransitionAirspeedProgressHasRecentSample(&filter, 200, 300)); + EXPECT_TRUE(mixerTransitionAirspeedProgressHasRecentSample(&filter, 399, 300)); + EXPECT_FALSE(mixerTransitionAirspeedProgressHasRecentSample(&filter, 400, 300)); + + mixerTransitionResetAirspeedProgressFilter(&filter); + EXPECT_FALSE(mixerTransitionAirspeedProgressHasRecentSample(&filter, 200, 300)); +} + +TEST(MixerTransitionPolicyTest, FwToMcAirspeedProgressUsesTheSameSpikeFilter) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.0f; + bool startCaptured = false; + float startAirspeedCmS = 0.0f; + const float samples[] = { 1400.0f, 1200.0f, 1000.0f, 800.0f }; + + for (uint32_t i = 0; i < 4; i++) { + const mixerTransitionHotSwitchProgress_t raw = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 800, + true, + samples[i], + startCaptured, + startAirspeedCmS, + i * 100, + 5000); + startCaptured = raw.transitionStartAirspeedCaptured; + startAirspeedCmS = raw.transitionStartAirspeedCmS; + accepted = mixerTransitionResolveHandoffProgress( + true, raw.usedAirspeed, accepted, raw.progress, i * 100, 300, 100, &filter); + } + + EXPECT_FLOAT_EQ(0.0f, accepted); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 1.0f, 400, 300, 100, &filter); + EXPECT_NEAR(1.0f / 3.0f, accepted, 0.0001f); +} + +TEST(MixerTransitionPolicyTest, MotorRampProgressFallsBackToFullWhenDisabledOrZeroRamp) +{ + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeMotorRampProgress(false, 500, 100)); + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeMotorRampProgress(true, 0, 100)); + EXPECT_FLOAT_EQ(0.40f, mixerTransitionComputeMotorRampProgress(true, 500, 200)); +} + +TEST(MixerTransitionPolicyTest, PostSwitchMotorOutputBlendIsBounded) +{ + EXPECT_EQ(1800, mixerTransitionBlendCapturedMotorOutput(1800, 1000, 0.0f)); + EXPECT_EQ(1400, mixerTransitionBlendCapturedMotorOutput(1800, 1000, 0.5f)); + EXPECT_EQ(1000, mixerTransitionBlendCapturedMotorOutput(1800, 1000, 1.0f)); + + EXPECT_EQ(1000, mixerTransitionBlendCapturedMotorOutput(1000, 1800, 0.0f)); + EXPECT_EQ(1400, mixerTransitionBlendCapturedMotorOutput(1000, 1800, 0.5f)); + EXPECT_EQ(1800, mixerTransitionBlendCapturedMotorOutput(1000, 1800, 1.0f)); + + EXPECT_EQ(1800, mixerTransitionBlendCapturedMotorOutput(1800, 1000, -0.25f)); + EXPECT_EQ(1000, mixerTransitionBlendCapturedMotorOutput(1800, 1000, 1.25f)); +} + +TEST(MixerTransitionPolicyTest, DirectionNoneKeepsScalesAndHotSwitchIdle) +{ + const mixerTransitionScaleState_t scales = mixerTransitionComputeScales( + true, + MIXERAT_DIRECTION_NONE, + 0.20f, + 0.30f, + 0.40f, + 0.50f, + 0.25f); + + EXPECT_FLOAT_EQ(1.0f, scales.pusherScale); + EXPECT_FLOAT_EQ(1.0f, scales.liftScale); + EXPECT_FLOAT_EQ(1.0f, scales.mcAuthorityScale); + EXPECT_FLOAT_EQ(1.0f, scales.fwAuthorityScale); + EXPECT_FLOAT_EQ(1.0f, scales.blendToFw); + + const mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_NONE, + 1500, + true, + 1800.0f, + false, + 0.0f, + 300, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_FALSE(progress.usedAirspeed); + EXPECT_FALSE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(0.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, HotSwitchFallsBackToTimerWhenAirspeedIsUnavailable) +{ + const mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1500, + false, + 0.0f, + false, + 0.0f, + 300, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_FALSE(progress.usedAirspeed); + EXPECT_FALSE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(0.30f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, HotSwitchUsesAirspeedWhenAvailableForMcToFw) +{ + const mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1500, + true, + 1800.0f, + false, + 0.0f, + 0, + 1000); + + EXPECT_TRUE(progress.readyForHotSwitch); + EXPECT_TRUE(progress.usedAirspeed); + EXPECT_FLOAT_EQ(1.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, AirspeedEndpointUsesTheConfirmedProgressWindow) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.0f; + + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 200, 300, 100, &filter); + EXPECT_FALSE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); + + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 300, 300, 100, &filter); + EXPECT_TRUE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); + EXPECT_FLOAT_EQ(1.0f, accepted); +} + +TEST(MixerTransitionPolicyTest, AirspeedEndpointRejectsABriefDropBelowThreshold) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.0f; + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 1.0f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.95f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 1.0f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 1.0f, 300, 300, 100, &filter); + EXPECT_FALSE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 1.0f, 400, 300, 100, &filter); + EXPECT_FALSE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 1.0f, 500, 300, 100, &filter); + EXPECT_TRUE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); +} + +TEST(MixerTransitionPolicyTest, HotSwitchWithoutAirspeedCompletesImmediatelyWhenTimerIsZero) +{ + const mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1500, + false, + 0.0f, + false, + 0.0f, + 0, + 0); + + EXPECT_TRUE(progress.readyForHotSwitch); + EXPECT_FALSE(progress.usedAirspeed); + EXPECT_FALSE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(1.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, HotSwitchCapturesAndReusesStartAirspeedForFwToMc) +{ + mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1600.0f, + false, + 0.0f, + 0, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_TRUE(progress.usedAirspeed); + EXPECT_TRUE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(1600.0f, progress.transitionStartAirspeedCmS); + EXPECT_FLOAT_EQ(0.0f, progress.progress); + + progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1300.0f, + progress.transitionStartAirspeedCaptured, + progress.transitionStartAirspeedCmS, + 0, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_FLOAT_EQ(0.50f, progress.progress); + + progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 900.0f, + progress.transitionStartAirspeedCaptured, + progress.transitionStartAirspeedCmS, + 0, + 1000); + + EXPECT_TRUE(progress.readyForHotSwitch); + EXPECT_FLOAT_EQ(1.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, FwToMcProgressLeavesEndpointIfSpeedRisesAfterStartingBelowThreshold) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 850, + true, + 800.0f, + false, + 0.0f, + 0, + 5000); + + EXPECT_TRUE(progress.readyForHotSwitch); + EXPECT_FLOAT_EQ(1.0f, progress.progress); + + float accepted = mixerTransitionResolveHandoffProgress( + false, progress.usedAirspeed, 0.0f, progress.progress, 0, 300, 100, &filter); + + progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 850, + true, + 1000.0f, + progress.transitionStartAirspeedCaptured, + progress.transitionStartAirspeedCmS, + 100, + 5000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_LT(progress.progress, 1.0f); + + accepted = mixerTransitionResolveHandoffProgress( + false, progress.usedAirspeed, accepted, progress.progress, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 300, 300, 100, &filter); + EXPECT_FALSE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); + + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 400, 300, 100, &filter); + EXPECT_FALSE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); + accepted = mixerTransitionResolveHandoffProgress(false, true, accepted, 1.0f, 500, 300, 100, &filter); + EXPECT_TRUE(mixerTransitionAirspeedProgressEndpointConfirmed(&filter)); +} + +TEST(MixerTransitionPolicyTest, AirspeedEndpointProgressMatchesTheHotSwitchConditionInBothDirections) +{ + const float airspeedsCmS[] = { 0.0f, 800.0f, 850.0f, 851.0f, 1000.0f, 1600.0f }; + + for (const float airspeedCmS : airspeedsCmS) { + const mixerTransitionHotSwitchProgress_t toFw = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 850, + true, + airspeedCmS, + false, + 0.0f, + 0, + 5000); + EXPECT_EQ(toFw.readyForHotSwitch, toFw.progress >= 1.0f); + + const mixerTransitionHotSwitchProgress_t toMcFromAbove = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 850, + true, + airspeedCmS, + true, + 1600.0f, + 0, + 5000); + EXPECT_EQ(toMcFromAbove.readyForHotSwitch, toMcFromAbove.progress >= 1.0f); + + const mixerTransitionHotSwitchProgress_t toMcFromBelow = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 850, + true, + airspeedCmS, + true, + 800.0f, + 0, + 5000); + EXPECT_EQ(toMcFromBelow.readyForHotSwitch, toMcFromBelow.progress >= 1.0f); + } +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskCapturesOldLiftAndNewPusherButNotSharedTiltMotors) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; // shared tilt motor + currentMixer[1].throttle = 1.0f; // old lift motor + targetMixer[0].throttle = 1.0f; // shared tilt motor + targetMixer[2].throttle = 1.0f; // FW pusher appears after switch + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 3, + currentMixer, + targetMixer); + + EXPECT_EQ((1U << 1) | (1U << 2), fadeMask.motorMask); + EXPECT_EQ((1U << 2), fadeMask.toCurrentMotorMask); + EXPECT_EQ(0U, fadeMask.fastToCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskCapturesOldPusherAndNewLiftWhenReturningToMultirotor) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; // shared tilt motor + currentMixer[2].throttle = 1.0f; // FW pusher disappears after switch + targetMixer[0].throttle = 1.0f; // shared tilt motor + targetMixer[1].throttle = 1.0f; // MC lift motor + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_MC, + false, + 3, + currentMixer, + targetMixer); + + EXPECT_EQ((1U << 1) | (1U << 2), fadeMask.motorMask); + EXPECT_EQ((1U << 1), fadeMask.toCurrentMotorMask); + EXPECT_EQ((1U << 1), fadeMask.fastToCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, NewMcLiftOnlyUsesBoundedOutputHandoffDuration) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + targetMixer[0].throttle = 1.0f; + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 4000, + MIXERAT_DIRECTION_TO_MC, + false, + 1, + currentMixer, + targetMixer); + + EXPECT_EQ((1U << 0), fadeMask.motorMask); + EXPECT_EQ((1U << 0), fadeMask.toCurrentMotorMask); + EXPECT_EQ((1U << 0), fadeMask.fastToCurrentMotorMask); + EXPECT_EQ(MIXER_TRANSITION_MC_LIFT_OUTPUT_HANDOFF_MAX_MS, + mixerTransitionComputePostSwitchFadeDurationMs(4000, fadeMask.motorMask, fadeMask.fastToCurrentMotorMask)); +} + +TEST(MixerTransitionPolicyTest, ExistingPusherFadeKeepsConfiguredDurationAlongsideNewMcLift) +{ + const uint16_t motorMask = (1U << 1) | (1U << 2); + const uint16_t fastToCurrentMotorMask = (1U << 1); + + EXPECT_EQ(4000U, mixerTransitionComputePostSwitchFadeDurationMs( + 4000, motorMask, fastToCurrentMotorMask)); +} + +TEST(MixerTransitionPolicyTest, PusherScaleDoesNotFollowAirspeedHandoffProgress) +{ + const mixerTransitionScaleState_t early = mixerTransitionComputeScales( + true, MIXERAT_DIRECTION_TO_FW, 0.20f, 0.30f, 0.40f, 0.20f, 0.50f); + const mixerTransitionScaleState_t late = mixerTransitionComputeScales( + true, MIXERAT_DIRECTION_TO_FW, 0.20f, 0.30f, 0.40f, 0.90f, 0.50f); + + EXPECT_FLOAT_EQ(early.pusherScale, late.pusherScale); + EXPECT_LT(late.liftScale, early.liftScale); +} + +TEST(MixerTransitionPolicyTest, AirspeedProgressSlewsAConfirmedStepInsteadOfJumping) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.0f; + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.60f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.60f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.60f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.60f, 300, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.0f, accepted); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.60f, 350, 300, 100, &filter); + EXPECT_NEAR(1.0f / 6.0f, accepted, 0.0001f); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.60f, 450, 300, 100, &filter); + EXPECT_NEAR(0.50f, accepted, 0.0001f); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.60f, 550, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.60f, accepted); +} + +TEST(MixerTransitionPolicyTest, AirspeedProgressRetargetsSmoothlyWhileAlreadySlewing) +{ + mixerTransitionAirspeedProgressFilter_t filter = {}; + float accepted = 0.0f; + + // Establish an initial sustained target without applying a full progress + // step at the end of its confirmation window. + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.20f, 0, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.20f, 100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.20f, 200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.20f, 300, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.20f, 400, 300, 100, &filter); + EXPECT_FLOAT_EQ(0.20f, accepted); + + // A higher target needs its own stable window. Once it is confirmed, the + // existing slew continues from 0.20 instead of restarting or jumping. + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.90f, 500, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.90f, 600, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.90f, 700, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.90f, 800, 300, 100, &filter); + EXPECT_NEAR(0.20f + 1.0f / 3.0f, accepted, 0.0001f); + + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.90f, 900, 300, 100, &filter); + EXPECT_NEAR(0.20f + 2.0f / 3.0f, accepted, 0.0001f); + + // The same rule applies in reverse: a lower target is only accepted once + // the complete recent window is lower, then the existing slew reverses. + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.10f, 1000, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.10f, 1100, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.10f, 1200, 300, 100, &filter); + accepted = mixerTransitionResolveHandoffProgress(true, true, accepted, 0.10f, 1300, 300, 100, &filter); + EXPECT_NEAR(0.20f + 1.0f / 3.0f, accepted, 0.0001f); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskStaysEmptyWhenDynamicScalingIsDisabled) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; + targetMixer[1].throttle = 1.0f; + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + false, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 2, + currentMixer, + targetMixer); + + EXPECT_EQ(0U, fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskStaysEmptyForSharedTiltOnlyConfiguration) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; + targetMixer[0].throttle = 1.0f; + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 1, + currentMixer, + targetMixer); + + EXPECT_EQ(0U, fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, AutoServoInputMayDecreaseDuringFwToMc) +{ + EXPECT_EQ(100, mixerTransitionUpdateServoInput( + 500, + false, + true, + true, + false, + false, + 0.20f)); +} diff --git a/src/test/unit/mixer_transition_scenarios_unittest.cc b/src/test/unit/mixer_transition_scenarios_unittest.cc new file mode 100644 index 00000000000..95004bca159 --- /dev/null +++ b/src/test/unit/mixer_transition_scenarios_unittest.cc @@ -0,0 +1,400 @@ +#include + +extern "C" { +#include "flight/mixer_transition_logic.h" +} + +namespace { + +struct ManualTransitionScenario { + explicit ManualTransitionScenario(const bool transitionToFixedWing) + : transitionToFw(transitionToFixedWing) + { + } + + void setTransitionMode(const bool active, const bool manualControllerConfigured, const bool clearSession = false) + { + const bool risingEdge = active && !transitionModeActive; + const bool fallingEdge = !active && transitionModeActive; + + sessionMode = mixerTransitionUpdateManualSessionMode( + sessionMode, + risingEdge, + fallingEdge, + manualControllerConfigured, + clearSession); + transitionModeActive = active; + } + + bool autoControllerEnabled(const bool manualControllerConfigured) const + { + return mixerTransitionManualControllerEnabled(manualControllerConfigured, sessionMode); + } + + int16_t updateServo( + const bool transitionMixingActive, + const bool autoTransitionActive, + const bool postSwitchFadeToFwActive, + const float blendToFw) + { + servoInput = mixerTransitionUpdateServoInput( + servoInput, + sessionMode == MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + transitionMixingActive, + autoTransitionActive, + postSwitchFadeToFwActive, + transitionToFw, + blendToFw); + return servoInput; + } + + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + int16_t servoInput = 0; + bool transitionModeActive = false; + bool transitionToFw = true; +}; + +float computeTransitionServoBlendForStep( + const bool dynamicMixerEnabled, + const mixerProfileATDirection_e direction, + const uint16_t scaleRampTimeMs, + const uint32_t elapsedMs, + const uint32_t transitionTimerMs) +{ + (void)transitionTimerMs; + + return mixerTransitionComputeServoBlendToFw( + false, + true, + true, + false, + dynamicMixerEnabled, + direction, + scaleRampTimeMs, + elapsedMs); +} + +} // namespace + +TEST(MixerTransitionScenarioTest, LegacyManualMcToFwSessionStaysLegacyAcrossProfileHotSwitch) +{ + ManualTransitionScenario scenario(true); + + scenario.setTransitionMode(true, false); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_LEGACY, scenario.sessionMode); + EXPECT_FALSE(scenario.autoControllerEnabled(false)); + EXPECT_EQ(500, scenario.updateServo(true, false, false, 0.18f)); + + // A hot-switch onto a profile that enables the new controller must not + // retroactively convert the already-running legacy session. + scenario.setTransitionMode(true, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_LEGACY, scenario.sessionMode); + EXPECT_FALSE(scenario.autoControllerEnabled(true)); + EXPECT_EQ(500, scenario.updateServo(true, true, false, 0.02f)); + + scenario.setTransitionMode(false, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, scenario.sessionMode); + EXPECT_EQ(0, scenario.updateServo(false, false, false, 0.0f)); +} + +TEST(MixerTransitionScenarioTest, AutoMcToFwSessionStaysMonotonicAcrossAirspeedDropoutAndHotSwitch) +{ + ManualTransitionScenario scenario(true); + + scenario.setTransitionMode(true, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); + + mixerTransitionHotSwitchProgress_t hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + true, + 320.0f, + false, + 0.0f, + 0, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(125, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + true, + MIXERAT_DIRECTION_TO_FW, + 1000, + 250, + 1000))); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + true, + 650.0f, + false, + 0.0f, + 0, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(275, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + true, + MIXERAT_DIRECTION_TO_FW, + 1000, + 550, + 1000))); + + // After a profile hot-switch, the active auto session must survive even if + // the target profile itself has the manual controller disabled. + scenario.setTransitionMode(true, false); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); + EXPECT_TRUE(scenario.autoControllerEnabled(false)); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + false, + 0.0f, + false, + 0.0f, + 500, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(275, scenario.updateServo( + true, + scenario.autoControllerEnabled(false), + false, + computeTransitionServoBlendForStep( + true, + MIXERAT_DIRECTION_TO_FW, + 1000, + 500, + 1000))); + + // Even if the raw blend source drops during the same control cycle as the + // hot-switch, the MC->FW servo source must not move backwards. + EXPECT_EQ(275, scenario.updateServo( + false, + scenario.autoControllerEnabled(false), + false, + 0.20f)); + EXPECT_EQ(500, scenario.updateServo( + false, + scenario.autoControllerEnabled(false), + true, + 0.20f)); + + scenario.setTransitionMode(false, false); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, scenario.sessionMode); + EXPECT_EQ(0, scenario.updateServo(false, false, false, 0.0f)); +} + +TEST(MixerTransitionScenarioTest, AutoMcToFwSessionKeepsLegacyServoEndpointWhenDynamicMixerIsDisabled) +{ + ManualTransitionScenario scenario(true); + + scenario.setTransitionMode(true, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); + + mixerTransitionHotSwitchProgress_t hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + true, + 320.0f, + false, + 0.0f, + 0, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + false, + MIXERAT_DIRECTION_TO_FW, + 1000, + 0, + 1000))); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1000, + false, + 0.0f, + false, + 0.0f, + 500, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + false, + MIXERAT_DIRECTION_TO_FW, + 1000, + 500, + 1000))); +} + +TEST(MixerTransitionScenarioTest, AutoFwToMcSessionUsesCapturedAirspeedAndReturnsTowardMultirotor) +{ + ManualTransitionScenario scenario(false); + + scenario.setTransitionMode(true, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_AUTO, scenario.sessionMode); + + mixerTransitionHotSwitchProgress_t hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1600.0f, + false, + 0.0f, + 0, + 1000); + EXPECT_TRUE(hotSwitchProgress.transitionStartAirspeedCaptured); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + true, + MIXERAT_DIRECTION_TO_MC, + 1000, + 0, + 1000))); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1300.0f, + hotSwitchProgress.transitionStartAirspeedCaptured, + hotSwitchProgress.transitionStartAirspeedCmS, + 0, + 1000); + EXPECT_FALSE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(125, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + true, + MIXERAT_DIRECTION_TO_MC, + 1000, + 750, + 1000))); + + hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 900.0f, + hotSwitchProgress.transitionStartAirspeedCaptured, + hotSwitchProgress.transitionStartAirspeedCmS, + 0, + 1000); + EXPECT_TRUE(hotSwitchProgress.readyForHotSwitch); + EXPECT_EQ(0, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + true, + MIXERAT_DIRECTION_TO_MC, + 1000, + 1000, + 1000))); + + scenario.setTransitionMode(false, true); + EXPECT_EQ(MIXER_TRANSITION_MANUAL_SESSION_NONE, scenario.sessionMode); + EXPECT_EQ(0, scenario.updateServo(false, false, false, 0.0f)); +} + +TEST(MixerTransitionScenarioTest, PusherHotSwitchFadeScenarioCapturesOnlyNonSharedMotors) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + ManualTransitionScenario scenario(true); + + currentMixer[0].throttle = 1.0f; // shared tilt motor + currentMixer[1].throttle = 1.0f; // lift motor disappears after switch + targetMixer[0].throttle = 1.0f; // shared tilt motor + targetMixer[2].throttle = 1.0f; // FW pusher appears after switch + + scenario.setTransitionMode(true, true); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + true, + MIXERAT_DIRECTION_TO_FW, + 1000, + 1000, + 1000))); + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 3, + currentMixer, + targetMixer); + + EXPECT_EQ((1U << 1) | (1U << 2), fadeMask.motorMask); + EXPECT_EQ((1U << 2), fadeMask.toCurrentMotorMask); + EXPECT_EQ(500, scenario.updateServo( + false, + scenario.autoControllerEnabled(true), + true, + 0.0f)); +} + +TEST(MixerTransitionScenarioTest, SharedTiltOnlyHotSwitchScenarioNeedsNoFadeAndStaysAtFwEndpoint) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + ManualTransitionScenario scenario(true); + + currentMixer[0].throttle = 1.0f; + targetMixer[0].throttle = 1.0f; + + scenario.setTransitionMode(true, true); + EXPECT_EQ(500, scenario.updateServo( + true, + scenario.autoControllerEnabled(true), + false, + computeTransitionServoBlendForStep( + true, + MIXERAT_DIRECTION_TO_FW, + 1000, + 1000, + 1000))); + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 1, + currentMixer, + targetMixer); + + EXPECT_EQ(0U, fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); + + // With shared tilt motors there is no separate fade mask, but the FW + // endpoint still must not fall back during the handoff cycle. + EXPECT_EQ(500, scenario.updateServo( + false, + scenario.autoControllerEnabled(true), + false, + 0.0f)); +} diff --git a/src/test/unit/navigation_vtol_mission_logic_unittest.cc b/src/test/unit/navigation_vtol_mission_logic_unittest.cc new file mode 100644 index 00000000000..a2a254d9f33 --- /dev/null +++ b/src/test/unit/navigation_vtol_mission_logic_unittest.cc @@ -0,0 +1,528 @@ +#include + +extern "C" { +#include "navigation/navigation_vtol_mission_logic.h" +} + +TEST(NavigationVtolMissionLogicTest, ReadyWhenAllPreconditionsAreMet) +{ + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_READY, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + true, + true, + true, + false)); +} + +TEST(NavigationVtolMissionLogicTest, TemporaryRuntimeConditionsWait) +{ + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_WAIT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + true, + true, + true, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_WAIT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + false, + true, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_WAIT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + false, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_WAIT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + true, + true, + true, + true)); +} + +TEST(NavigationVtolMissionLogicTest, HardSafetyAndConfigurationProblemsReject) +{ + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_REJECT, + navMissionVtolTransitionPreconditionDisposition( + false, + false, + false, + true, + true, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_REJECT, + navMissionVtolTransitionPreconditionDisposition( + true, + true, + false, + true, + true, + true, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_REJECT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + true, + false, + true, + false)); + + EXPECT_EQ(NAV_MISSION_VTOL_PRECONDITION_REJECT, + navMissionVtolTransitionPreconditionDisposition( + true, + false, + false, + true, + true, + true, + false, + false)); +} + +TEST(NavigationVtolMissionLogicTest, StartValidationRejectsBadTargetProfile) +{ + EXPECT_EQ(NAV_MISSION_VTOL_START_VALIDATION_REJECT, + navMissionVtolTransitionStartValidation( + false, + true)); +} + +TEST(NavigationVtolMissionLogicTest, StartValidationUsesFailActionWhenRequestCannotStart) +{ + EXPECT_EQ(NAV_MISSION_VTOL_START_VALIDATION_FAIL_ACTION, + navMissionVtolTransitionStartValidation( + true, + false)); +} + +TEST(NavigationVtolMissionLogicTest, StartValidationAllowsValidTransitionStart) +{ + EXPECT_EQ(NAV_MISSION_VTOL_START_VALIDATION_READY, + navMissionVtolTransitionStartValidation( + true, + true)); +} + +TEST(NavigationVtolMissionLogicTest, SuccessfulMissionToFixedWingAdvancesPlainWaypointAfterClimb) +{ + const bool acceptedForAdvance = navMissionTransitionWaypointAcceptedForAdvance( + true, + true, + 100, + 12000.0f, + 12000.0f); + + EXPECT_TRUE(navMissionShouldAdvanceWaypointAfterTransition( + acceptedForAdvance, + true, + 100, + 12680.0f, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, SuccessfulMissionToMultirotorAdvancesPlainWaypointAfterClimb) +{ + const bool acceptedForAdvance = navMissionTransitionWaypointAcceptedForAdvance( + true, + true, + 100, + 12000.0f, + 12000.0f); + + EXPECT_TRUE(navMissionShouldAdvanceWaypointAfterTransition( + acceptedForAdvance, + true, + 100, + 13290.0f, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MissionTransitionDoesNotAdvanceBelowEnforcedAltitude) +{ + const bool acceptedForAdvance = navMissionTransitionWaypointAcceptedForAdvance( + true, + true, + 100, + 12000.0f, + 12000.0f); + + EXPECT_FALSE(navMissionShouldAdvanceWaypointAfterTransition( + acceptedForAdvance, + true, + 100, + 11899.0f, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MissionTransitionStartedBelowEnforcedAltitudeDoesNotLatchWaypoint) +{ + EXPECT_FALSE(navMissionTransitionWaypointAcceptedForAdvance( + true, + true, + 100, + 11899.0f, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MissionTransitionKeepsLegacyAdvanceWhenAltitudeEnforcementIsDisabled) +{ + const bool acceptedForAdvance = navMissionTransitionWaypointAcceptedForAdvance( + true, + true, + 0, + 11000.0f, + 12000.0f); + + EXPECT_TRUE(navMissionShouldAdvanceWaypointAfterTransition( + acceptedForAdvance, + true, + 0, + 11000.0f, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MissionTransitionDoesNotAdvanceHoldWaypointOrAbort) +{ + const bool holdWaypointAccepted = navMissionTransitionWaypointAcceptedForAdvance( + true, + false, + 100, + 12000.0f, + 12000.0f); + + EXPECT_FALSE(navMissionShouldAdvanceWaypointAfterTransition( + holdWaypointAccepted, + true, + 100, + 12680.0f, + 12000.0f)); + + EXPECT_FALSE(navMissionShouldAdvanceWaypointAfterTransition( + true, + false, + 100, + 12680.0f, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, ForcedFwToMcProfileSwitchAdvancesAcceptedWaypoint) +{ + const bool acceptedForAdvance = navMissionTransitionWaypointAcceptedForAdvance( + true, + true, + 300, + 12042.0f, + 12000.0f); + + EXPECT_TRUE(navMissionTransitionReachedTargetProfile(true, true)); + EXPECT_TRUE(navMissionShouldAdvanceWaypointAfterTransition( + acceptedForAdvance, + navMissionTransitionReachedTargetProfile(true, true), + 300, + 13975.0f, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, AbortedTransitionWithoutProfileSwitchDoesNotAdvanceWaypoint) +{ + EXPECT_FALSE(navMissionTransitionReachedTargetProfile(true, false)); + EXPECT_FALSE(navMissionShouldAdvanceWaypointAfterTransition( + true, + navMissionTransitionReachedTargetProfile(true, false), + 300, + 13975.0f, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, FwToMcHoldWaypointCapturesBeforeMissionResume) +{ + EXPECT_TRUE(navMissionTransitionShouldCaptureBeforeWaypointResume( + true, + true, + true, + true, + false)); + + EXPECT_FALSE(navMissionTransitionShouldRebaseWaypointBeforeResume( + true, + true, + false, + true)); +} + +TEST(NavigationVtolMissionLogicTest, McToFwHoldWaypointRebasesWithoutMultirotorCapture) +{ + EXPECT_FALSE(navMissionTransitionShouldCaptureBeforeWaypointResume( + true, + true, + false, + false, + false)); + + EXPECT_TRUE(navMissionTransitionShouldRebaseWaypointBeforeResume( + true, + true, + false, + false)); +} + +TEST(NavigationVtolMissionLogicTest, WaypointAdvanceAndAbortedTransitionSkipResumeHandling) +{ + EXPECT_FALSE(navMissionTransitionShouldCaptureBeforeWaypointResume( + true, + true, + true, + true, + true)); + EXPECT_FALSE(navMissionTransitionShouldRebaseWaypointBeforeResume( + true, + true, + true, + false)); + + EXPECT_FALSE(navMissionTransitionShouldCaptureBeforeWaypointResume( + true, + false, + true, + true, + false)); + EXPECT_FALSE(navMissionTransitionShouldRebaseWaypointBeforeResume( + true, + false, + false, + false)); +} + +TEST(NavigationVtolMissionLogicTest, MissionTransitionSuppressesWaypointFallbackOnlyWhileInProgress) +{ + EXPECT_TRUE(navMissionVtolTransitionSuppressesWaypointRestartGuard(true)); + EXPECT_TRUE(navMissionVtolTransitionSuppressesWaypointFallbackToRth(true)); + EXPECT_TRUE(navMissionVtolTransitionHoldsWaypointSelector(true, true)); + + EXPECT_FALSE(navMissionVtolTransitionSuppressesWaypointRestartGuard(false)); + EXPECT_FALSE(navMissionVtolTransitionSuppressesWaypointFallbackToRth(false)); + EXPECT_FALSE(navMissionVtolTransitionHoldsWaypointSelector(false, true)); + EXPECT_FALSE(navMissionVtolTransitionHoldsWaypointSelector(true, false)); +} + +TEST(NavigationVtolMissionLogicTest, MultirotorWaypointHoldsForEnforcedAltitudeInsideRadius) +{ + EXPECT_TRUE(navMissionShouldHoldMultirotorWaypointForAltitude( + true, + false, + 100, + 250.0f, + 250.0f, + 500, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MultirotorWaypointKeepsAltitudeHoldWhenStartedInsideRadius) +{ + EXPECT_TRUE(navMissionShouldHoldMultirotorWaypointForAltitude( + true, + false, + 100, + 650.0f, + 250.0f, + 500, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MultirotorWaypointDoesNotHoldWhenAltitudeReached) +{ + EXPECT_FALSE(navMissionShouldHoldMultirotorWaypointForAltitude( + true, + false, + 100, + 250.0f, + 250.0f, + 500, + 99.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MultirotorWaypointAltitudeHoldIsDisabledWithoutEnforceAltitude) +{ + EXPECT_FALSE(navMissionShouldHoldMultirotorWaypointForAltitude( + true, + false, + 0, + 250.0f, + 250.0f, + 500, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MultirotorWaypointAltitudeHoldDoesNotAffectFixedWing) +{ + EXPECT_FALSE(navMissionShouldHoldMultirotorWaypointForAltitude( + false, + false, + 100, + 250.0f, + 250.0f, + 500, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MultirotorWaypointAltitudeHoldWaitsForHorizontalRadius) +{ + EXPECT_FALSE(navMissionShouldHoldMultirotorWaypointForAltitude( + true, + false, + 100, + 501.0f, + 501.0f, + 500, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, MultirotorWaypointKeepsAltitudeHoldAfterEnteringRadius) +{ + EXPECT_TRUE(navMissionUpdateMultirotorWaypointAltitudeEnforceActive( + true, + false, + false, + 100, + 250.0f, + 650.0f, + 500)); + + EXPECT_TRUE(navMissionShouldHoldMultirotorWaypointForAltitude( + true, + true, + 100, + 650.0f, + 650.0f, + 500, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, FirstMultirotorWaypointCanEnforceAltitudeBeforeRadius) +{ + EXPECT_TRUE(navMissionUpdateMultirotorWaypointAltitudeEnforceActive( + true, + false, + true, + 100, + 1200.0f, + 1200.0f, + 500)); + + EXPECT_TRUE(navMissionShouldHoldMultirotorWaypointForAltitude( + true, + true, + 100, + 1200.0f, + 1200.0f, + 500, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, NonFirstMultirotorWaypointStillWaitsForRadius) +{ + EXPECT_FALSE(navMissionUpdateMultirotorWaypointAltitudeEnforceActive( + true, + false, + false, + 100, + 1200.0f, + 1200.0f, + 500)); +} + +TEST(NavigationVtolMissionLogicTest, FirstGeoWaypointAltitudeForceRequiresMultirotor) +{ + EXPECT_TRUE(navMissionShouldForceFirstGeoWaypointAltitudeFromStart( + true, + true)); + + EXPECT_FALSE(navMissionShouldForceFirstGeoWaypointAltitudeFromStart( + true, + false)); + + EXPECT_FALSE(navMissionShouldForceFirstGeoWaypointAltitudeFromStart( + false, + true)); +} + +TEST(NavigationVtolMissionLogicTest, FirstMultirotorWaypointHoldsXyUntilAltitudeReached) +{ + EXPECT_TRUE(navMissionShouldHoldFirstMultirotorWaypointXyForAltitude( + true, + true, + 100, + 12000.0f)); + + EXPECT_FALSE(navMissionShouldHoldFirstMultirotorWaypointXyForAltitude( + true, + true, + 100, + 99.0f)); + + EXPECT_FALSE(navMissionShouldHoldFirstMultirotorWaypointXyForAltitude( + true, + false, + 100, + 12000.0f)); + + EXPECT_FALSE(navMissionShouldHoldFirstMultirotorWaypointXyForAltitude( + false, + true, + 100, + 12000.0f)); +} + +TEST(NavigationVtolMissionLogicTest, ActiveMultirotorAltitudeEnforcementKeepsWaypointAltitude) +{ + EXPECT_FLOAT_EQ(12000.0f, navMissionMultirotorWaypointAltitudeTarget( + true, + true, + 12000.0f, + 250.0f)); + + EXPECT_FLOAT_EQ(250.0f, navMissionMultirotorWaypointAltitudeTarget( + true, + false, + 12000.0f, + 250.0f)); + + EXPECT_FLOAT_EQ(250.0f, navMissionMultirotorWaypointAltitudeTarget( + false, + true, + 12000.0f, + 250.0f)); +} diff --git a/src/test/unit/vtol_mc_protection_logic_unittest.cc b/src/test/unit/vtol_mc_protection_logic_unittest.cc new file mode 100644 index 00000000000..820e465b248 --- /dev/null +++ b/src/test/unit/vtol_mc_protection_logic_unittest.cc @@ -0,0 +1,481 @@ +#include + +extern "C" { +#include "navigation/navigation_vtol_mc_protection_logic.h" +} + +TEST(VtolMcProtectionLogicTest, DetectsOnlyPairedMultirotorVtolMcMode) +{ + EXPECT_TRUE(vtolMcProtectionDetectVtolMcMode(true, true, true)); + EXPECT_FALSE(vtolMcProtectionDetectVtolMcMode(false, true, true)); + EXPECT_FALSE(vtolMcProtectionDetectVtolMcMode(true, false, true)); + EXPECT_FALSE(vtolMcProtectionDetectVtolMcMode(true, true, false)); +} + +TEST(VtolMcProtectionLogicTest, DefaultOffKeepsLegacyThrottleBounds) +{ + const vtolMcProtectionThrottleBounds_t bounds = vtolMcProtectionComputeThrottleBounds( + false, + 1000, + 1500, + 2000, + 15); + + EXPECT_EQ(1000, bounds.min); + EXPECT_EQ(2000, bounds.max); + EXPECT_FALSE(bounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, AppliesThrottleReserveBeforePidBounds) +{ + const vtolMcProtectionThrottleBounds_t bounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1500, + 2000, + 15); + + EXPECT_EQ(1150, bounds.min); + EXPECT_EQ(1850, bounds.max); + EXPECT_FALSE(bounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, ShrinksReserveToKeepHoverThrottleReachable) +{ + const vtolMcProtectionThrottleBounds_t lowHoverBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1100, + 2000, + 20); + + EXPECT_EQ(1100, lowHoverBounds.min); + EXPECT_EQ(1800, lowHoverBounds.max); + EXPECT_TRUE(lowHoverBounds.reserveShrunk); + + const vtolMcProtectionThrottleBounds_t highHoverBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1900, + 2000, + 20); + + EXPECT_EQ(1200, highHoverBounds.min); + EXPECT_EQ(1900, highHoverBounds.max); + EXPECT_TRUE(highHoverBounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, ThrottleBoundsHandleDisabledReserveAndInvalidRange) +{ + const vtolMcProtectionThrottleBounds_t zeroReserveBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1500, + 2000, + 0); + + EXPECT_EQ(1000, zeroReserveBounds.min); + EXPECT_EQ(2000, zeroReserveBounds.max); + EXPECT_FALSE(zeroReserveBounds.reserveShrunk); + + const vtolMcProtectionThrottleBounds_t invalidRangeBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1500, + 1500, + 1400, + 20); + + EXPECT_EQ(1500, invalidRangeBounds.min); + EXPECT_EQ(1400, invalidRangeBounds.max); + EXPECT_FALSE(invalidRangeBounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, SettleTimerRequiresContinuousStableConditions) +{ + vtolMcProtectionSettleState_t state = {}; + + EXPECT_EQ(1000, VTOL_MC_SETTLE_TIME_MS); + EXPECT_EQ(3000, VTOL_MC_FALLBACK_SETTLE_TIME_MS); + EXPECT_EQ(VTOL_MC_SETTLE_TIME_MS, vtolMcProtectionSettleTimeMs(true)); + EXPECT_EQ(VTOL_MC_FALLBACK_SETTLE_TIME_MS, vtolMcProtectionSettleTimeMs(false)); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 100)); + EXPECT_EQ(0, state.elapsedMs); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 600)); + EXPECT_EQ(500, state.elapsedMs); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, false, VTOL_MC_SETTLE_TIME_MS, 700)); + EXPECT_EQ(0, state.elapsedMs); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 800)); + EXPECT_TRUE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 1800)); +} + +TEST(VtolMcProtectionLogicTest, SettleTimerSaturatesElapsedTime) +{ + vtolMcProtectionSettleState_t state = {}; + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, UINT16_MAX, 1)); + EXPECT_TRUE(vtolMcProtectionUpdateSettleState(&state, true, UINT16_MAX, 70000)); + EXPECT_EQ(UINT16_MAX, state.elapsedMs); +} + +TEST(VtolMcProtectionLogicTest, SettleConditionsUseDerivedThresholds) +{ + EXPECT_EQ(75, vtolMcProtectionHorizontalSettleSpeedCmS(0)); + EXPECT_EQ(50, vtolMcProtectionHorizontalSettleSpeedCmS(50)); + EXPECT_EQ(50, vtolMcProtectionVerticalSettleSpeedCmS(50)); + EXPECT_EQ(100, vtolMcProtectionVerticalSettleSpeedCmS(500)); + EXPECT_EQ(200, vtolMcProtectionSettleAttitudeLimitDeciDeg(35)); + EXPECT_EQ(150, vtolMcProtectionSettleAttitudeLimitDeciDeg(15)); + + EXPECT_TRUE(vtolMcProtectionSettleConditionsMet(70.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMet(80.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMet(70.0f, -120.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMet(70.0f, -50.0f, 250, 75, 100, 200)); +} + +TEST(VtolMcProtectionLogicTest, VelocityFallbackSettleUsesVerticalSpeedAndAttitudeOnly) +{ + EXPECT_FALSE(vtolMcProtectionUsingVelocityFallback(true, true)); + EXPECT_FALSE(vtolMcProtectionUsingVelocityFallback(false, false)); + EXPECT_TRUE(vtolMcProtectionUsingVelocityFallback(false, true)); + + EXPECT_TRUE(vtolMcProtectionSettleConditionsMetWithFallback( + false, true, 500.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + false, false, 0.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + false, true, 0.0f, -120.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + false, true, 0.0f, -50.0f, 250, 75, 100, 200)); + + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + true, true, 500.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + true, false, 0.0f, 0.0f, 0, 75, 100, 200)); +} + +TEST(VtolMcProtectionLogicTest, LandingCaptureRadiusCapsLargeWaypointRadius) +{ + EXPECT_EQ(100, vtolMcProtectionLandingCaptureRadiusCm(10000)); + EXPECT_EQ(80, vtolMcProtectionLandingCaptureRadiusCm(80)); +} + +TEST(VtolMcProtectionLogicTest, PositionCaptureDoesNotOverrideNavigationOrPilotTarget) +{ + EXPECT_TRUE(vtolMcProtectionPositionCaptureAllowed(true, true, false, false)); + EXPECT_FALSE(vtolMcProtectionPositionCaptureAllowed(false, true, false, false)); + EXPECT_FALSE(vtolMcProtectionPositionCaptureAllowed(true, false, false, false)); + EXPECT_FALSE(vtolMcProtectionPositionCaptureAllowed(true, true, true, false)); + EXPECT_FALSE(vtolMcProtectionPositionCaptureAllowed(true, true, false, true)); +} + +TEST(VtolMcProtectionLogicTest, PositionCaptureSetsBrakingTargetOnlyOnEntry) +{ + EXPECT_FALSE(vtolMcProtectionCaptureShouldSetTarget(false, false)); + EXPECT_TRUE(vtolMcProtectionCaptureShouldSetTarget(false, true)); + EXPECT_FALSE(vtolMcProtectionCaptureShouldSetTarget(true, true)); + EXPECT_FALSE(vtolMcProtectionCaptureShouldSetTarget(true, false)); +} + +TEST(VtolMcProtectionLogicTest, LandingDescentDoesNotInvalidateApprovedSettle) +{ + // Normal vertical descent is deliberately not an input to this gate. + EXPECT_FALSE(vtolMcProtectionLandingDescentNeedsResettle( + true, true, true, 120.0f, 150, 75, 200)); + EXPECT_FALSE(vtolMcProtectionLandingDescentNeedsResettle( + true, true, false, 1000.0f, 150, 75, 200)); + + EXPECT_TRUE(vtolMcProtectionLandingDescentNeedsResettle( + true, true, true, 151.0f, 150, 75, 200)); + EXPECT_TRUE(vtolMcProtectionLandingDescentNeedsResettle( + true, true, true, 20.0f, 201, 75, 200)); +} + +TEST(VtolMcProtectionLogicTest, LandingResettleRequiresProtectionAndPriorApproval) +{ + EXPECT_FALSE(vtolMcProtectionLandingDescentNeedsResettle( + false, true, true, 300.0f, 300, 75, 200)); + EXPECT_FALSE(vtolMcProtectionLandingDescentNeedsResettle( + true, false, true, 300.0f, 300, 75, 200)); +} + +TEST(VtolMcProtectionLogicTest, SoftAltitudeRelaxationOnlyAppliesDuringCaptureOrTransition) +{ + EXPECT_TRUE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, false, 120.0f, 75, false)); + EXPECT_TRUE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, false, 20.0f, 75, true)); + + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + false, true, true, true, false, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, false, true, true, false, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, false, true, false, false, false, 120.0f, 75, false)); + EXPECT_TRUE(vtolMcProtectionShouldRelaxAltitude( + true, true, false, true, false, false, false, 120.0f, 75, true)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, false, false, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, true, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, true, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, true, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, false, 75.0f, 75, false)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpKeepsLegacyBehaviorForNonVtol) +{ + EXPECT_TRUE(vtolMcProtectionLandingBumpAllowed( + false, + false, + false, + 1000.0f, + false, + -300.0f, + -300.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpAllowsTrustedNearGroundTouchdown) +{ + EXPECT_TRUE(vtolMcProtectionLandingBumpAllowed( + true, + false, + true, + VTOL_MC_LANDING_BUMP_SAFE_AGL_CM, + false, + -300.0f, + -300.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpBlocksTrustedHighAgl) +{ + EXPECT_FALSE(vtolMcProtectionLandingBumpAllowed( + true, + true, + true, + VTOL_MC_LANDING_BUMP_SAFE_AGL_CM + 1.0f, + true, + -20.0f, + -50.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpBlocksVtolHighAltitudeFastDescent) +{ + EXPECT_FALSE(vtolMcProtectionLandingBumpAllowed( + true, + true, + false, + 1000.0f, + true, + -170.0f, + -150.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpAllowsVtolFinalSlowDescent) +{ + EXPECT_TRUE(vtolMcProtectionLandingBumpAllowed( + true, + true, + false, + 1000.0f, + true, + -60.0f, + -50.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingBumpBlocksVtolWithoutTouchdownContext) +{ + EXPECT_FALSE(vtolMcProtectionLandingBumpAllowed( + true, + false, + false, + 1000.0f, + true, + -20.0f, + 0.0f, + 75.0f, + 100.0f)); + + EXPECT_FALSE(vtolMcProtectionLandingBumpAllowed( + true, + true, + false, + 1000.0f, + false, + 0.0f, + -50.0f, + 75.0f, + 100.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeThrottleDropUsesSmallBoundedReduction) +{ + EXPECT_EQ(40, vtolMcProtectionLandingProbeThrottleDrop(1000, 1200)); + EXPECT_EQ(50, vtolMcProtectionLandingProbeThrottleDrop(1000, 1500)); + EXPECT_EQ(100, vtolMcProtectionLandingProbeThrottleDrop(1000, 2500)); + EXPECT_EQ(0, vtolMcProtectionLandingProbeThrottleDrop(1000, 1000)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeThrottleRampsDownFromStart) +{ + EXPECT_EQ(1500, vtolMcProtectionLandingProbeThrottle(1500, 1000, 1500, 1000, 1000)); + EXPECT_EQ(1475, vtolMcProtectionLandingProbeThrottle(1500, 1000, 1500, 1000, 1150)); + EXPECT_EQ(1450, vtolMcProtectionLandingProbeThrottle(1500, 1000, 1500, 1000, 1300)); + EXPECT_EQ(1000, vtolMcProtectionLandingProbeThrottle(0, 1000, 1500, 1000, 1150)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeDetectsAirborneByAglDrop) +{ + EXPECT_TRUE(vtolMcProtectionLandingProbeAirborneResponse( + true, + 40.0f, + 30.0f, + false, + 0.0f, + 0.0f, + 1.0f)); + + EXPECT_FALSE(vtolMcProtectionLandingProbeAirborneResponse( + true, + 40.0f, + 35.0f, + false, + 0.0f, + 0.0f, + 1.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeDetectsAirborneByLowG) +{ + EXPECT_TRUE(vtolMcProtectionLandingProbeAirborneResponse( + false, + 0.0f, + 0.0f, + false, + 0.0f, + 0.0f, + VTOL_MC_LANDING_PROBE_LOW_G_THRESHOLD - 0.01f)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeDetectsAirborneByVerticalVelocityChange) +{ + EXPECT_TRUE(vtolMcProtectionLandingProbeAirborneResponse( + false, + 0.0f, + 0.0f, + true, + -20.0f, + -45.0f, + 1.0f)); + + EXPECT_TRUE(vtolMcProtectionLandingProbeAirborneResponse( + false, + 0.0f, + 0.0f, + true, + -50.0f, + -90.0f, + 1.0f)); +} + +TEST(VtolMcProtectionLogicTest, LandingProbeDoesNotReactWithoutDescentEvidence) +{ + EXPECT_FALSE(vtolMcProtectionLandingProbeAirborneResponse( + false, + 0.0f, + 0.0f, + true, + -20.0f, + -30.0f, + 1.0f)); +} + +TEST(VtolMcProtectionLogicTest, BailoutAngleLimitUsesBankAngleWithSafeClamps) +{ + EXPECT_EQ(450, vtolMcProtectionBailoutAngleLimitDeciDeg(20)); + EXPECT_EQ(500, vtolMcProtectionBailoutAngleLimitDeciDeg(35)); + EXPECT_EQ(600, vtolMcProtectionBailoutAngleLimitDeciDeg(80)); +} + +TEST(VtolMcProtectionLogicTest, ActiveProtectionSuppressesMulticopterBrakingMode) +{ + EXPECT_FALSE(vtolMcProtectionSuppressesMulticopterBrakingMode(false)); + EXPECT_TRUE(vtolMcProtectionSuppressesMulticopterBrakingMode(true)); +} + +TEST(VtolMcProtectionLogicTest, RthLandingYawSettleAssistRequiresSettledVtolMcLanding) +{ + EXPECT_TRUE(vtolMcProtectionRthLandingYawSettleAssistActive( + true, + true, + true, + false, + false)); + + EXPECT_FALSE(vtolMcProtectionRthLandingYawSettleAssistActive( + false, + true, + true, + false, + false)); + + EXPECT_FALSE(vtolMcProtectionRthLandingYawSettleAssistActive( + true, + false, + true, + false, + false)); + + EXPECT_FALSE(vtolMcProtectionRthLandingYawSettleAssistActive( + true, + true, + false, + false, + false)); + + EXPECT_FALSE(vtolMcProtectionRthLandingYawSettleAssistActive( + true, + true, + true, + true, + false)); + + EXPECT_FALSE(vtolMcProtectionRthLandingYawSettleAssistActive( + true, + true, + true, + false, + true)); +} + +TEST(VtolMcProtectionLogicTest, CommandShapingIsContinuousAndPreservesSign) +{ + EXPECT_FLOAT_EQ(1.0f, vtolMcProtectionCommandScaleForSpeed(250.0f)); + EXPECT_FLOAT_EQ(1.0f, vtolMcProtectionCommandScaleForSpeed(VTOL_MC_COMMAND_SHAPE_START_CM_S)); + EXPECT_FLOAT_EQ(0.75f, vtolMcProtectionCommandScaleForSpeed(550.0f)); + EXPECT_FLOAT_EQ(0.5f, vtolMcProtectionCommandScaleForSpeed(VTOL_MC_COMMAND_SHAPE_FULL_CM_S)); + EXPECT_FLOAT_EQ(0.5f, vtolMcProtectionCommandScaleForSpeed(900.0f)); + EXPECT_EQ(750, vtolMcProtectionCommandScalePermille(0.75f)); + + EXPECT_EQ(0, vtolMcProtectionApplyCommandScale(0, 0.5f)); + EXPECT_EQ(150, vtolMcProtectionApplyCommandScale(200, 0.75f)); + EXPECT_EQ(-150, vtolMcProtectionApplyCommandScale(-200, 0.75f)); + EXPECT_EQ(1, vtolMcProtectionApplyCommandScale(1, 0.5f)); + EXPECT_EQ(-1, vtolMcProtectionApplyCommandScale(-1, 0.5f)); +}