diff --git a/docs/ADSB.md b/docs/ADSB.md index cd7c0d9f244..5cbbb52516a 100644 --- a/docs/ADSB.md +++ b/docs/ADSB.md @@ -64,47 +64,6 @@ AT+SETTINGS=SAVE * in INAV configurator ports TAB set telemetry MAVLINK, and baudrate 115200 * https://pantsforbirds.com/adsbee-1090/quick-start/ -<<<<<<< HEAD ---- - ---- - -## Alert and Warning -The ADS-B warning/alert system supports two operating modes, controlled by the parameter osd_adsb_calculation_use_cpa (ON or OFF). - ---- - -### ADS-B Warning and Alert Messages (CPA Mode OFF) -The ADS-B warning/alert system supports two operating modes, controlled by the parameter **osd_adsb_calculation_use_cpa** (ON or OFF). - -When **osd_adsb_calculation_use_cpa = OFF**, the system evaluates only the **current distance between the aircraft and the UAV**. The aircraft with the **shortest distance** is always selected for monitoring. - -- If the aircraft enters the **warning zone** (`adsb_distance_warning`), the corresponding **OSD element is displayed**. -- If the aircraft enters the **alert zone** (`adsb_distance_alert`), the **OSD element starts blinking**, indicating a higher-priority alert. - -This mode therefore provides a simple proximity-based warning determined purely by real-time distance. - ---- - -### ADS-B Warning and Alert Messages (CPA Mode ON) - -When **osd_adsb_calculation_use_cpa = ON**, the system evaluates aircraft using the **Closest Point of Approach (CPA)** and predicted trajectories, not only the current distance. - -1. **Aircraft already inside the alert zone** - If one or more aircraft are currently inside the **alert zone** (`adsb_distance_alert`), the **closest aircraft** to the UAV is selected and the **OSD element blinks**. - -2. **Aircraft in the warning zone, none predicted to enter the alert zone** - If aircraft are present in the **warning zone** (`adsb_distance_warning`), but none of them are predicted to enter the **alert zone** (their CPA distance is greater than `adsb_distance_alert`), the **closest aircraft to the UAV** is selected and the **OSD element remains steady** (no blinking). - -3. **Aircraft in the warning zone, one predicted to enter the alert zone** - If at least one aircraft in the **warning zone** is predicted to enter the **alert zone**, that aircraft is selected and the **OSD element blinks**. - -4. **Aircraft in the warning zone, multiple predicted to enter the alert zone** - If multiple aircraft are predicted to enter the **alert zone**, the system selects the aircraft that will **reach the alert zone first**, and the **OSD element blinks**. - -![ADSB CPA_ON](assets/images/adsb-CPA-on.png) - - ## SoftRF settings SoftRF supports only MAVLink version 1. ``` @@ -150,4 +109,3 @@ When **osd_adsb_calculation_use_cpa = ON**, the system evaluates aircraft using If multiple aircraft are predicted to enter the **alert zone**, the system selects the aircraft that will **reach the alert zone first**, and the **OSD element blinks**. ![ADSB CPA_ON](assets/images/adsb-CPA-on.png) ->>>>>>> upstream/release/9.1 diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 685e17a4e70..ff5179f767b 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -6,21 +6,71 @@ 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 trusted pitot airspeed gets too low. During manual FW flight, INAV stays in MC until you deliberately command another manual profile change. During mission/RTH/failsafe, INAV keeps the current navigation task in MC after the safety switch. + +This behavior is controlled by `mixer_vtol_manualswitch_autotransition_controller`. +Turn it ON in both mixer profiles if you want the same switch behavior in both directions. +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: -The use of Transition Mode is recommended to enable further features and future developments like fail-safe support. Mapping motor to servo output, or servo with logic conditions is **not** recommended +- `MIXER 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. ## Servo @@ -50,13 +100,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,11 +122,274 @@ Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input It is also possible to set it as 4 state switch by adding FW(profile1) with transition on. +If `mixer_vtol_manualswitch_autotransition_controller = ON` and this overlap position is active: +- the smooth transition controller runs while `MIXER TRANSITION` stays ON +- direct `MIXER PROFILE 2` switching is deferred during that time +- when `MIXER TRANSITION` turns OFF, `MIXER PROFILE 2` again decides which stable mixer profile should be active + +This overlap style is supported too. + ## Automated Transition -This feature is mainly for RTH in a failsafe event. When set properly, model will use the FW mode to fly home efficiently, And land in the MC mode for easier landing. -`ON` for a mixer_profile\`s `mixer_automated_switch` means to schedule a Automated Transition when RTH head home(applies for MC mixer_profile) or RTH Land(applies for FW mixer_profile) is requested by navigation controller. -Set `mixer_automated_switch` to `ON` in mixer_profile for MC mode. Set `mixer_switch_trans_timer` in mixer_profile for MC mode for the time required to gain airspeed for your model before entering to FW mode. -When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Model will not perform automated transition at all. +This feature is mainly used for RTH and failsafe. +When set up correctly, the aircraft can use fixed-wing for the efficient flight home and then return to multicopter for easier landing. + +If `mixer_automated_switch = ON` in a mixer profile, INAV is allowed to run an automated transition when the navigation logic asks for it. + +- Use `mixer_automated_switch = ON` in the MC mixer profile if you want automated MC->FW transition during the head-home part of RTH. +- Use `mixer_automated_switch = ON` in the FW mixer profile if you want automated FW->MC transition for the landing part. +- Set `mixer_switch_trans_timer` in each profile to a sensible backup time for that direction. + +If `mixer_automated_switch = OFF` in all mixer profiles, automated VTOL transition is disabled. + +### Unified VTOL transition controller + +Manual `MIXER TRANSITION` and mission-requested VTOL transition both use the same internal transition controller. +That means the same airspeed checks, timer fallback, and smooth power changes are reused in both cases. + +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 pitot is not available, not healthy, or the threshold is set to `0`, INAV uses `mixer_switch_trans_timer` instead. +Ground speed is not used for transition completion/progress. + +The three timer settings do different jobs: + +- `mixer_switch_trans_timer` is the original VTOL transition timer. It is still the backup completion timer when trusted pitot airspeed is not being used. +- `mixer_vtol_transition_airspeed_timeout_ms` is only a maximum wait time for the required airspeed while pitot is still usable. It does not complete the transition by itself; it aborts that airspeed-controlled attempt. +- If pitot becomes unavailable during transition, INAV stops using the airspeed timeout and falls back to `mixer_switch_trans_timer`. +- For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). + +### Smooth power changes during transition (optional) + +When `mixer_vtol_transition_dynamic_mixer = ON`, INAV can smoothly change: + +- forward motor power, +- lift motor power, +- multicopter stabilisation strength, +- fixed-wing control strength. + +Default is OFF to preserve existing behavior. +With it ON, 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. + +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. +- 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. + - After the FW->MC profile switch, a forward motor that is not used by the MC profile keeps moving to idle and cannot be reintroduced by FW throttle. + - `INPUT_MIXER_TRANSITION` uses this ramp when `mixer_vtol_transition_dynamic_mixer = ON`; with dynamic mixer OFF it keeps the older fixed transition endpoint behavior. + - If a profile switch or direct switch changes a transition-linked servo output, INAV captures the current servo output and starts a fresh full window from this value. + - `= 0` (default): those time-based power changes happen immediately. +- This timer does not decide when the transition completes. +- Airspeed-based control scaling: + - in MC->FW, lift power reduction, MC stabilisation reduction, and FW control increase follow airspeed when pitot is usable. + - in FW->MC, FW control reduction follows airspeed when pitot is usable. + - with valid pitot airspeed, they follow transition progress based on airspeed. + - without trusted pitot, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). +- In FW->MC, forward motor removal, lift power return, and MC motor stabilisation return use the time-based motor ramp so they do not wait for airspeed to fall first. + +Example: + +- `mixer_switch_trans_timer = 50` (5s fallback completion timer) +- `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 pitot stops being usable, airspeed-linked control scaling falls back to `mixer_switch_trans_timer`, +- transition completion still uses airspeed when pitot is working, +- backup completion time is still `5s` if pitot is not usable. + +### 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 the high byte +- `debug[2]` = packed transition flags: + - bits 0-1: transition direction (`0=NONE`, `1=TO_FW`, `2=TO_MC`) + - bit2: auto-transition controller active + - 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 (planned for INAV 7.1) TailSitter is supported by add a 90deg offset to the board alignment. Set the board aliment normally in the mixer_profile for FW mode(`set platform_type = AIRPLANE`), The motor trust axis should be same direction as the airplane nose. Then, in the mixer_profile for takeoff and landing set `tailsitter_orientation_offset = ON ` to apply orientation offset. orientation offset will also add a 45deg orientation offset. diff --git a/docs/Navigation.md b/docs/Navigation.md index 4d7b9740d91..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 cbc526adb33..364f7c713de 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 | | --- @@ -3191,7 +3193,7 @@ This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly ### mixer_automated_switch -If set to on, This mixer_profile will try to switch to another mixer_profile when 1.RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius on mixer_profile is a MULTIROTOR or TRICOPTER platform_type. 2. RTH landing is requested on this mixer_profile is a AIRPLANE platform_type +Allows navigation code to request VTOL mixer profile changes from this profile. In legacy RTH this lets MC/TRICOPTER profiles switch toward FW when returning from far away, and lets AIRPLANE profiles switch toward MC for RTH landing. With the VTOL auto-transition controller, mission waypoints can also request MC/FW transitions through their USER action. Enable this in each profile where NAV is allowed to change the mixer profile. | Default | Min | Max | | --- | --- | --- | @@ -3211,7 +3213,7 @@ If enabled, control_profile_index will follow mixer_profile index. Set to OFF(de ### mixer_switch_trans_timer -If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. +Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, INAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, airspeed-linked power and control changes also fall back to this timer whenever trusted pitot is not usable. | Default | Min | Max | | --- | --- | --- | @@ -3219,6 +3221,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 pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, INAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 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 trusted pitot airspeed 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. @@ -3981,7 +4023,7 @@ Deadband for heading trajectory PID controller. When heading error is below the ### nav_land_detect_sensitivity -Changes sensitivity of landing detection. Higher values increase speed of detection but also increase risk of false detection. Default value should work in most cases. +Scales the generic landing detector velocity and gyro thresholds. `5` is nominal sensitivity: for MC this is about 100 cm/s horizontal and vertical speed plus 4 deg/s average pitch/roll gyro rate. Higher values relax these thresholds and can detect landing sooner, but increase false-detect risk. VTOL MC landing detection also uses additional hard vertical-speed and throttle-probe safety gates that this setting does not bypass. | Default | Min | Max | | --- | --- | --- | @@ -4031,7 +4073,7 @@ Defines at what altitude the descent velocity should start to be `nav_land_minal ### nav_landing_bump_detection -Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS). +Allows landing detection to use a touchdown G-bump candidate when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS). For VTOL MC mode the G-bump is not an immediate disarm shortcut: trusted high AGL blocks it, and accepted candidates must still pass the VTOL MC throttle-probe confirmation. | Default | Min | Max | | --- | --- | --- | @@ -4636,6 +4678,67 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT --- +### nav_vtol_mission_transition_min_altitude_cm + +Do not start a mission-requested VTOL transition below this altitude [cm]. Set to 0 to disable the altitude check. 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. @@ -6996,6 +7099,88 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, --- +### vtol_fw_to_mc_auto_switch_airspeed_cm_s + +Extra low-speed fixed-wing safety fallback [cm/s]. If trusted pitot airspeed falls to this value or lower while in FW, INAV automatically starts FW->MC. In manual FW flight this requires `mixer_vtol_manualswitch_autotransition_controller` to be ON and keeps MC until the pilot deliberately commands another manual profile change. In mission/RTH/failsafe this requires `mixer_automated_switch` to be ON and keeps the current navigation task in MC after the safety switch, blocking automatic MC->FW re-entry for that navigation session. `vtol_transition_to_mc_max_airspeed_cm_s` still controls when the FW->MC transition is safe to complete. Set to 0 to disable. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 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 power used during transition, in percent. In MC->FW, lift power is reduced to this value as the transition progresses. In FW->MC, lift power starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_mc_authority_min_percent + +Lowest multicopter stabilisation used during transition, in percent. In MC->FW, active MC motor stabilisation is reduced to this value as the transition progresses. In FW->MC, target MC motor stabilisation starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### vtol_transition_to_fw_min_airspeed_cm_s + +Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. 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 pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 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..3c92b7b9783 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. @@ -174,6 +180,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 +251,7 @@ If you have set up the mixer as suggested in STEP1 and STEP2, you may have to de # STEP 5: Transition Mixing (Multi-Rotor Profile)(Recommended) ### Transition Mixing is typically useful in multi-copter profile to gain airspeed in prior to entering the fixed-wing profile. When the `MIXER TRANSITION` mode is activated, the associated motor or servo will move according to your configured Transition Mixing. -Please note that transition input is disabled when a navigation mode is activated. The use of Transition Mixing is necessary to enable additional features such as VTOL RTH with out stalling. +Please note that manual transition input is disabled when a navigation mode is active. Mission-authorized VTOL transition (via configured waypoint User Action) still works through the automated transition state. ## Servo 'Transition Mixing': Tilting rotor configuration. Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weight/rate according to your desired angle. This will allow tilting the motor for tilting rotor model. @@ -247,7 +259,7 @@ Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weigh ## Motor 'Transition Mixing': Dedicated forward motor configuration In motor mixer set: -- -2.0 < throttle < -1.0: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated. +- -2.0 <= throttle <= -1.05: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated. Use exactly `-1.000` only as a placeholder, not as a transition helper motor. - Airmode type should be set to "STICK_CENTER". Airmode type must NOT be set to "THROTTLE_THRESHOLD". If set to throttle threshold the (-) motor will spin until the throttle threshold is passed. ![Alt text](Screenshots/mixerprofile_4puls1_mix.png) @@ -257,32 +269,780 @@ 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. + +## 1. Manual switch auto transition + +Manual switch auto transition means that `MIXER TRANSITION` starts one complete transition. You no longer need to manually time the exact profile switch point. INAV starts the transition, waits for the configured speed or timer condition, then changes to the target mixer profile. + +This does not remove the older behavior. If `mixer_vtol_manualswitch_autotransition_controller = OFF`, manual transition stays as close as possible to the older INAV behavior. + +### Recommended first setup + +Configure both mixer profiles. This makes MC -> FW and FW -> MC behave consistently. + +Per-mixer-profile settings: + +``` +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: + +``` +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. -To enable this feature, type following command in cli +In the standard layout above, the transition position deliberately has both `MIXER PROFILE 2` and `MIXER TRANSITION` ON. While the auto-transition controller is enabled, the controller owns the profile switching until `MIXER TRANSITION` is turned OFF again. + +### Timer, pitot, and timeout examples + +`mixer_switch_trans_timer` is the backup completion timer. It is used when pitot airspeed is not configured, not trusted, or not available. + +Examples: + +- `mixer_switch_trans_timer = 30`: transition completes by timer after `3.0s` if pitot is not used. +- `mixer_switch_trans_timer = 50`: transition completes by timer after `5.0s` if pitot is not used. +- `vtol_transition_to_fw_min_airspeed_cm_s = 1300`: MC -> FW waits for `13 m/s` pitot airspeed when pitot is trusted. +- `vtol_transition_to_mc_max_airspeed_cm_s = 850`: FW -> MC waits until pitot airspeed falls to `8.5 m/s` or lower when pitot is trusted. +- `mixer_vtol_transition_airspeed_timeout_ms = 6500`: if pitot remains trusted but the requested airspeed is not reached within `6.5s`, that airspeed-controlled transition attempt is aborted. + +Important pitot behavior: + +- If pitot is trusted and a non-zero airspeed threshold is configured, INAV prefers airspeed for transition completion. +- The configured airspeed must be reached before `mixer_vtol_transition_airspeed_timeout_ms` expires. For example, with `vtol_transition_to_fw_min_airspeed_cm_s = 1300` and `mixer_vtol_transition_airspeed_timeout_ms = 6500`, MC -> FW must reach `13 m/s` within `6.5s`. +- If that timeout expires during a manual transition, the transition attempt is aborted and INAV does not force the target profile switch from that timeout. +- If that timeout expires during a mission transition, mission retry/failure handling is used: retry can run if `nav_vtol_transition_retry_on_airspeed_timeout = ON`; otherwise the configured mission fail action is used. +- If pitot becomes unavailable during the transition, INAV falls back to `mixer_switch_trans_timer`. +- Ground speed is not used to decide transition completion. +- `mixer_vtol_transition_airspeed_timeout_ms` does not complete a transition. It only stops an airspeed-controlled attempt that is taking too long. + +Optional low-speed protection: -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_fw_to_mc_auto_switch_airspeed_cm_s = 750 ``` -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 set, fixed-wing flight automatically starts FW -> MC when trusted pitot airspeed drops to `7.5 m/s` or lower. Set it to `0` to disable this protection. + +This is a safety fallback for cases where fixed-wing flight no longer looks safe, for example a pusher problem or another failure that prevents the aircraft from keeping enough airspeed. + +It can be used in two contexts: + +- During manual FW flight, it requires `mixer_vtol_manualswitch_autotransition_controller = ON`. After the switch, INAV stays in MC until you deliberately command another manual profile change. +- During mission, RTH, and failsafe RTH, it is a navigation-owned safety transition. It requires `mixer_automated_switch = ON` and a valid MC target profile. After it switches to MC, INAV keeps the current navigation task in MC and does not automatically start another MC -> FW transition for that navigation session, even if a later mission waypoint requests FW. + +This setting decides when the emergency FW -> MC fallback starts. `vtol_transition_to_mc_max_airspeed_cm_s` still decides when the FW -> MC profile switch is safe to complete during an airspeed-controlled FW -> MC transition. + +## 2. Manual switch auto transition with dynamic scaling + +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 2or1 -set mixer_switch_trans_timer = 30 # 3s, 3000ms +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: + ``` -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_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 ``` -mixer_profile 1or2 + +What dynamic scaling changes: + +- MC -> FW can smoothly bring in the forward motor before the profile switch. +- MC -> FW can reduce lift motor power and MC motor stabilisation while fixed-wing control is increased. +- FW -> MC can remove the forward motor while lift motors and MC motor stabilisation come back. +- FW -> MC can reduce fixed-wing control as MC control comes back. +- Transition-linked servos continue from their current output if the profile switch would otherwise cause a step. + +What it does not change: + +- It does not decide by itself when the transition is complete. +- Completion still uses trusted pitot airspeed first, or `mixer_switch_trans_timer` when pitot is not used. +- With `mixer_vtol_transition_dynamic_mixer = OFF`, the old transition input behavior is preserved and the auto-controller only manages the profile switch timing. + +Value direction notes: + +- Lower `mixer_vtol_transition_scale_ramp_time_ms`: pusher, lift return, and tilt servo movement happen faster. Too low can still look abrupt. +- Higher `mixer_vtol_transition_scale_ramp_time_ms`: movement is gentler, but the aircraft spends longer with partial pusher/lift/servo authority. +- Lower `vtol_transition_lift_min_percent`: lift motors reduce more during MC -> FW. This can reduce drag/power use but gives less lift reserve. +- Higher `vtol_transition_lift_min_percent`: more lift is kept through transition. `100` keeps full lift power. +- Lower `vtol_transition_mc_authority_min_percent`: MC motor stabilisation is reduced more during MC -> FW. Use carefully on large VTOLs. +- Higher `vtol_transition_mc_authority_min_percent`: more MC stabilisation stays available. `100` keeps full MC stabilisation. +- Lower `vtol_transition_fw_authority_min_percent`: fixed-wing control starts more gently. Higher values bring fixed-wing control in more strongly from the start. +- Lower airspeed thresholds complete sooner; higher thresholds wait for more airspeed before switching profile. + +### `mixer_vtol_transition_scale_ramp_time_ms` + +This setting controls the time-based smooth movement for: + +- 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. + +### Percentage settings + +These settings are active only when `mixer_vtol_transition_dynamic_mixer = ON`. + +`vtol_transition_lift_min_percent` + +- Current purpose: sets the lowest lift motor power used during transition. +- MC -> FW: lift power is reduced toward this value. +- FW -> MC: lift power starts from this value and returns to full power. +- `100` keeps full lift power for the whole transition. + +Example: + +- `vtol_transition_lift_min_percent = 30` +- At halfway through the lift-power change, lift power is about `65%`. +- At the lowest point, lift power is `30%`, not zero. + +`vtol_transition_mc_authority_min_percent` + +- Current purpose: sets the lowest MC motor stabilisation strength during transition. +- MC -> FW: active MC motor stabilisation is reduced toward this value. +- FW -> MC: target MC motor stabilisation starts from this value and returns to full strength. +- During FW -> MC, this target MC stabilisation comes from the MC mixer profile and target MC PID preview, not from the active FW controller. + +Example: + +- `vtol_transition_mc_authority_min_percent = 20` +- MC motor stabilisation never goes below `20%` during the transition. + +`vtol_transition_fw_authority_min_percent` + +- Current purpose: sets the lowest fixed-wing stabilisation strength during transition. +- MC -> FW: fixed-wing control starts from this value and increases to full strength. +- FW -> MC: fixed-wing control is reduced toward this value. +- When `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules are configured in the MC profile, this setting also controls how strongly those target fixed-wing servo corrections are applied during MC -> FW. + +Example: + +- `vtol_transition_fw_authority_min_percent = 20` +- Fixed-wing control starts gently, then increases as the transition progresses. +- `100` means fixed-wing control is full strength for the whole transition. + +### Pusher configuration example + +For a pusher VTOL, the preferred smooth setup is: + +- In the FW profile, configure the pusher as a normal positive-throttle motor. +- In the MC profile, reserve the same motor index with a placeholder rule if that motor is not used in MC flight. +- Use `throttle = -1.000`, `roll = 0`, `pitch = 0`, `yaw = 0` for the placeholder if Configurator removes zero-throttle motor rules. + +Example shape: + +``` +# MC profile: reserve motor 5 as a placeholder +mixer_profile 2 +mmix 4 -1.000 0.000 0.000 0.000 + +# FW profile: motor 5 is the real forward motor +mixer_profile 1 +mmix 4 1.000 0.000 0.000 0.000 +``` + +What happens with dynamic scaling ON: + +- MC -> FW: motor 5 starts from idle and smoothly reaches the requested FW throttle over `mixer_vtol_transition_scale_ramp_time_ms`. +- FW -> MC: motor 5 smoothly moves back to idle over `mixer_vtol_transition_scale_ramp_time_ms`. +- If motor 5 is not used by the destination profile after the switch, INAV keeps moving it toward idle instead of stopping it in one step. + +The older helper rule still works: + +``` +mmix 4 -1.200 0.000 0.000 0.000 +``` + +That older style spins the pusher at a fixed helper power only while `MIXER TRANSITION` is active. It can still be useful for legacy setups, but it is not the recommended setup for the smooth auto-transition controller. + +### Tilt servo configuration example + +Tilt servos commonly use: + +- `INPUT_MAX` (`29`) as a fixed offset +- `INPUT_MIXER_TRANSITION` (`38`) as the transition movement +- `INPUT_STABILIZED_YAW` (`2`) for tricopter/tiltmotor yaw correction + +Example based on a tricopter/tiltmotor style setup: + +``` +mixer_profile 2 +set platform_type = TRICOPTER + +smix reset +smix 0 4 2 -50 0 -1 # yaw correction on servo 4 +smix 1 5 2 -50 0 -1 # yaw correction on servo 5 +smix 2 4 29 87 0 -1 # fixed tilt offset on servo 4 +smix 3 5 29 -87 0 -1 # fixed tilt offset on servo 5 +smix 4 4 38 -45 0 -1 # transition tilt movement on servo 4 +smix 5 5 38 45 0 -1 # transition tilt movement on servo 5 +``` + +Behavior: + +- `INPUT_MAX` stays constant. +- With dynamic scaling OFF, `INPUT_MIXER_TRANSITION` keeps the older fixed transition value while transition mode is active. +- With dynamic scaling ON, `INPUT_MIXER_TRANSITION` moves from `0` to `500` over `mixer_vtol_transition_scale_ramp_time_ms`. +- During MC -> FW, `INPUT_MIXER_TRANSITION` must not move backwards when the FW profile is selected. +- During FW -> MC, it must also continue smoothly if the transition is aborted or reversed. +- If the destination profile does not own the same tilt servo output, INAV keeps the current servo output and moves it toward the destination output instead of briefly jumping through a middle/default position. + +Example: + +- MC tilt position: `90 degrees` +- transition tilt position from source 38: `45 degrees` +- FW profile final position: `0 degrees` +- `mixer_vtol_transition_scale_ramp_time_ms = 1200` + +Expected movement: + +- The servo moves from `90` toward `45` over about `1.2s`. +- If the profile switch then changes the final target from `45` to `0`, the servo continues from its current output and moves toward `0` over a fresh `1.2s`. + +### Optional fixed-wing control preview on MC profile + +If your aircraft has control surfaces that should start helping before the profile switch, add `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` rules in the MC mixer profile. + +Useful inputs: + +- `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: + +``` +set nav_vtol_mission_transition_user_action = USER1 +set nav_vtol_mission_transition_min_altitude_cm = 1200 +set nav_vtol_transition_retry_on_airspeed_timeout = ON +set nav_vtol_transition_fail_action_mc_to_fw = POSH +set nav_vtol_transition_fail_action_fw_to_mc = LOITER +save +``` + +How waypoint USER selection works: + +- selected USER bit clear (`0`) means target MC profile +- selected USER bit set (`1`) means target FW profile +- every navigable waypoint should intentionally set or clear that USER bit +- if the aircraft is already in the requested profile, INAV continues without starting a transition + +Example mission: + +- WP1: USER1 not set, climb in MC near the takeoff area +- WP2: USER1 set, request MC -> FW transition +- WP3: USER1 set, continue fixed-wing navigation +- LAND or final approach waypoint: USER1 not set, request FW -> MC before landing + +For MC -> FW mission transition, INAV uses a straight acceleration segment. It does not try to loiter to build airspeed. Normal waypoint advancement is paused until the transition is finished. + +### Altitude settings that matter in missions + +`nav_vtol_mission_transition_min_altitude_cm` + +- Current purpose: minimum altitude before a mission-requested VTOL transition may start. +- New transition use: prevents a mission from starting MC -> FW too low. +- `0` disables this gate. + +Example: + +- `nav_vtol_mission_transition_min_altitude_cm = 1200` +- Mission MC -> FW transition waits until current altitude is at least `12m` above the navigation reference. + +`nav_wp_enforce_altitude` + +- Existing purpose: controls whether a waypoint is considered reached only by horizontal distance, or also by altitude. +- New VTOL mission use: helps prevent the aircraft from accepting a transition waypoint horizontally while still far below the requested waypoint altitude. + +Examples: + +- `nav_wp_enforce_altitude = 0`: waypoint can be accepted by horizontal radius even if altitude is still not reached. This is faster, but can be risky for VTOL transition missions. +- Example with `nav_wp_enforce_altitude = 0`: if WP1 is directly above home at `120m`, the aircraft can be inside the horizontal waypoint radius immediately after takeoff. The waypoint may be accepted before the aircraft climbs to `120m`, so the mission can continue to the next action too early. +- `nav_wp_enforce_altitude = 100`: waypoint must be within about `1m` of the target altitude before it is considered reached. +- `nav_wp_enforce_altitude = 300`: waypoint must be within about `3m`; this is more relaxed and can be useful when altitude estimates are noisy. + +`nav_wp_radius` + +- Existing purpose: horizontal radius used to decide whether a normal waypoint has been reached. +- VTOL mission impact: if this is very large, a waypoint can be accepted early. Use `nav_wp_enforce_altitude` and `nav_vtol_mission_transition_min_altitude_cm` to avoid starting a transition before the aircraft is high enough. +- Landing safety use: VTOL MC landing settle uses a stricter internal landing capture radius based on `min(nav_wp_radius, 100cm)`. + +Examples: + +- `nav_wp_radius = 200`: waypoint accepted within `2m` horizontally. +- `nav_wp_radius = 1000`: waypoint accepted within `10m`; useful for fast FW navigation, but too loose by itself for deciding that a VTOL has settled at a landing point. + +### Mission transition failure behavior + +If transition cannot start because of temporary runtime conditions, INAV waits instead of immediately failing the mission. Temporary conditions include sensor calibration still in progress, navigation position temporarily unusable, heading temporarily unusable, or another transition already active. + +Hard safety/configuration problems are treated as errors or use the configured fail action where applicable. Examples include disarmed state, failsafe, missing mixer-profile mode setup, or no valid profile switch path. + +MC -> FW fail action: + +- `IDLE`: stop mission navigation and leave the pilot to recover +- `POSH`: enter position hold if possible +- `RTH`: return to home +- `EMERGENCY_LANDING`: start emergency landing behavior + +FW -> MC fail action: + +- `IDLE`: stop mission navigation and leave the pilot to recover +- `LOITER`: loiter in fixed-wing if possible +- `RTH`: return to home +- `EMERGENCY_LANDING`: start emergency landing behavior +- `FORCE_SWITCH`: force the target mixer switch even though the normal transition condition failed + +Use `FORCE_SWITCH` only when you have tested the airframe carefully. It is intended as a last-resort choice for setups where staying in FW is more dangerous than switching. + +`nav_vtol_transition_retry_on_airspeed_timeout = ON` allows one retry after an airspeed timeout. If the retry also fails, the configured fail action is used. + +## 4. Adding VTOL stabilisation + +A VTOL in MC mode is not always equivalent to a normal multicopter. Large wings can still create lift and drag while the aircraft is braking, yawing, descending, or correcting altitude. That can turn normal MC commands into unexpected climb, sink, roll, pitch, or yaw coupling. + +The VTOL MC protection feature is disabled by default: + ``` -4. Save your settings. type `save` in cli. +set vtol_mc_protection_mode = OFF +``` + +This means: + +- normal multicopters do not change +- fixed-wing mode does not change +- existing VTOL behavior does not change unless you enable the feature + +### Protection modes + +`vtol_mc_protection_mode = OFF` + +- Current behavior: no new VTOL MC protection. +- Use this when comparing against older behavior. + +`vtol_mc_protection_mode = NAV` + +- Adds VTOL MC protections during navigation and automatic-throttle operation. +- Applies only when INAV detects an active multicopter-like profile and another configured profile is fixed-wing-like. +- Does not affect fixed-wing flight. +- Does not affect normal non-VTOL multicopters. + +With `NAV`, INAV can: + +- reserve throttle range for attitude stabilisation before altitude controller anti-windup limits are applied +- keep hover throttle inside the safe range, shrinking the reserve if necessary +- damp horizontal speed before fully settling position-hold behavior +- relax altitude target capture while the aircraft is bleeding speed or transitioning in MC +- require a more stable landing condition before starting/describing landing as complete +- use a conservative recovery path if attitude becomes excessive during automatic-throttle VTOL MC navigation + +`vtol_mc_protection_mode = NAV_AND_STABILIZED` + +- 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. -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 VTOL MC protection enabled, INAV is more tolerant while the aircraft is still settling: +- NAV capture damps velocity first instead of immediately forcing a hard final hold point. +- Soft altitude capture follows current altitude more gently while horizontal speed is being reduced. +- During auto transition while the active side is MC, the same protection can reduce altitude-control aggression. +- Once the aircraft is stable, normal altitude/position behavior resumes. + +This is meant to cover the case where the wings affect MC behavior during braking, yawing, and transition. + +## 5. Landing detection setup + +Landing detection is safety-critical for VTOL. A large VTOL wing can make the aircraft bounce in ground effect, and barometric altitude alone cannot prove that the aircraft is on the ground. + +The new VTOL MC landing logic keeps the normal INAV landing detector, but adds extra checks before VTOL MC can report `LANDING_DETECTED`. + +### Basic landing settings + +`nav_rth_allow_landing` + +- Existing purpose: controls whether RTH is allowed to land. +- Values are `NEVER`, `ALWAYS`, and `FS_ONLY`. +- If landing is not allowed, RTH will not intentionally descend to land. + +`nav_disarm_on_landing` + +- Existing purpose: allows automatic disarm after landing detection. +- If OFF, landing may be detected but the FC will not automatically disarm because of landing. + +`nav_auto_disarm_delay` + +- Existing purpose: delay after landing detection before automatic disarm. +- VTOL MC use: delay still applies, but VTOL MC must first pass its additional landing confirmation checks. + +`nav_land_detect_sensitivity` + +- Existing purpose: scales the generic landing detector velocity and gyro thresholds. +- At default `5`, MC landing detection uses about `100 cm/s` horizontal speed, `100 cm/s` vertical speed, and `4 deg/s` average pitch/roll gyro threshold. +- Higher values make detection easier and earlier, but increase false-detect risk. +- VTOL MC use: this setting can create a landing candidate, but it cannot bypass VTOL MC vertical-speed and throttle-probe confirmation. + +Examples: + +- `nav_land_detect_sensitivity = 5`: conservative default. +- `nav_land_detect_sensitivity = 7`: detects more easily, useful if the craft bounces on touchdown, but test carefully. +- `nav_land_detect_sensitivity = 10`: much more relaxed; not recommended for initial VTOL testing because it can create landing candidates while still airborne. + +`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 the high byte. +- `debug[2]`: packed flags. Important bits include direction, controller active, transition input active, airspeed path active, profile switch done, abort state, current/next profile index, mission active, failsafe active, direct profile switch active, target preview mode, after-switch smoothing active, and legacy manual session active. +- `debug[3]`: main transition progress x1000 (`0..1000`). +- `debug[4]`: pusher/forward motor scale x1000 (`0..1000`). +- `debug[5]`: lift motor scale x1000 (`0..1000`). +- `debug[6]`: packed MC/FW stabilisation scales. Low 16 bits are MC stabilisation scale x1000, high 16 bits are FW control scale x1000. +- `debug[7]`: packed progress values. Bits `0..9` are airspeed-linked scaling progress, bits `10..19` are motor ramp progress, and bits `20..29` are after-switch smoothing progress. + +`VTOL_MC_PROTECT` debug channels: + +- `debug[0]`: flags bitmask. Bits show protection configured, VTOL MC detected, NAV protection active, ANGLE/HORIZON protection active, NAV capture active, landing settle active, bailout active, throttle reserve shrunk, soft altitude capture active, roll/pitch/yaw command shaped, and velocity fallback used. +- `debug[1]`: safe throttle minimum. +- `debug[2]`: safe throttle maximum. +- `debug[3]`: protected throttle. +- `debug[4]`: horizontal speed [cm/s]. +- `debug[5]`: vertical speed [cm/s]. +- `debug[6]`: max absolute roll/pitch attitude [deci-degrees]. +- `debug[7]`: capture/landing/bailout settle elapsed time [ms], or command scale x1000 when command shaping is active, otherwise `1000`. + +OSD system messages: + +INAV shows VTOL transition progress in the normal OSD system message field, including analog/MSP DisplayPort OSD and DJI HD OSD message output. + +Typical messages include: + +- `VTOL MANUAL TO FW` or `VTOL MANUAL TO MC`: manual switch transition is running. +- `VTOL MISSION TO FW` or `VTOL MISSION TO MC`: waypoint USER action requested a mission transition. +- `VTOL RTH TO FW`: RTH requested MC -> FW before flying home. +- `VTOL LAND TO MC`: RTH or mission landing requested FW -> MC before MC landing. +- `VTOL LOW SPD TO MC`: low-speed protection requested FW -> MC as a safety fallback. +- `VTOL RETRY SCAN`: mission/RTH MC -> FW timed out on airspeed and INAV is scanning headings for a better airspeed direction. +- `VTOL RETRY ALIGN`: INAV is turning to the best heading found by the retry scan before trying again. +- `VTOL FINISHING SWITCH`: the target profile is already active and old propulsion output is being moved to its safe final value. +- `VTOL AIRSPEED TIMEOUT`: the requested pitot airspeed was not reached before `mixer_vtol_transition_airspeed_timeout_ms`. +- `VTOL TRANSITION ABORTED`: the transition was cancelled before the profile switch. +- `VTOL TRANSITION DONE`: the transition finished. + +Per-mixer-profile settings: + +- `mixer_automated_switch`: existing RTH use is to allow NAV to switch between MC and FW for return/landing. New mission use is to allow waypoint USER actions to request MC/FW transitions. +- `mixer_switch_trans_timer`: existing legacy transition timer in deciseconds. New auto-transition use is the fallback completion timer when pitot airspeed is not used or not trusted. +- `mixer_vtol_transition_dynamic_mixer`: new optional smooth power/control scaling. OFF keeps old transition motor/servo behavior; ON allows smooth pusher, lift, MC stabilisation, FW control, and transition-servo movement. +- `mixer_vtol_manualswitch_autotransition_controller`: new manual switch controller. OFF keeps older manual behavior; ON makes `MIXER TRANSITION` start one complete transition. +- `mixer_vtol_transition_airspeed_timeout_ms`: new airspeed wait limit. It does not complete transition; it aborts an airspeed-controlled attempt that takes too long while pitot remains trusted. +- `mixer_vtol_transition_scale_ramp_time_ms`: new time used for smooth pusher/lift/transition-servo movement. It also controls how long transition-linked servo outputs continue from their current position after a direct switch or abort. + +Global VTOL transition settings: + +- `vtol_transition_to_fw_min_airspeed_cm_s`: new preferred MC -> FW completion threshold when pitot is trusted. `0` uses the timer path. +- `vtol_transition_to_mc_max_airspeed_cm_s`: new preferred FW -> MC completion threshold when pitot is trusted. `0` uses the timer path. +- `vtol_fw_to_mc_auto_switch_airspeed_cm_s`: new low-speed fixed-wing safety fallback. When non-zero, FW flight can automatically start FW -> MC if trusted pitot airspeed falls too low. In manual FW flight it requires the manual auto-transition controller. In mission/RTH/failsafe it requires `mixer_automated_switch = ON` and keeps the current navigation task in MC after the switch. +- `vtol_transition_lift_min_percent`: new lowest lift motor power during dynamic transition scaling. `100` keeps full lift power. +- `vtol_transition_mc_authority_min_percent`: new lowest MC motor stabilisation strength during dynamic transition scaling. `100` keeps full MC stabilisation. +- `vtol_transition_fw_authority_min_percent`: new lowest FW control strength during dynamic transition scaling. It also scales optional target fixed-wing servo preview rules. +- `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 +1057,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 + +## Pitot-based transition logic (reference) + +When pitot is healthy/available, transition progress is airspeed-driven (not timer-driven). + +- MC -> FW: + - progress = `constrain(airspeed / to_fw_threshold, 0..1)` + - completion condition = `airspeed >= to_fw_threshold` + +- FW -> MC: + - capture `startAirspeed` when transition starts + - progress = `constrain((startAirspeed - airspeed) / (startAirspeed - to_mc_threshold), 0..1)` + - completion condition = `airspeed <= to_mc_threshold` + +Smooth transition power changes (`mixer_vtol_transition_dynamic_mixer = ON`) use this progress: + +- MC -> FW: + - forward motor power ramps `0 -> 1` + - lift motor power ramps `1 -> vtol_transition_lift_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 pitot-based transition progress whenever pitot is working. +In FW->MC, FW control reduction still prefers pitot-based transition progress, while forward motor removal, lift power return, and MC motor stabilisation return use the time-based motor ramp. +If pitot is not usable, the airspeed-linked changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). + +For legacy/helper transition motors (`-2.0 <= throttle <= -1.05`), output is interpolated from idle to target: + +`motor = idle + (target - idle) * pusherScale` + +where: +- `target = -mixerThrottle * 1000` +- `idle = throttleRangeMin` + +If pitot is unavailable/unhealthy, timer fallback is used (`mixer_switch_trans_timer`). + +For smooth auto-transition, the preferred forward motor setup is a normal positive-throttle rule in the FW mixer profile, with a placeholder on the same motor index in the MC mixer profile. Use `throttle = -1.000` for that placeholder if Configurator removes zero-throttle motor rules. If a helper such as `throttle = -1.200` is used, INAV 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 e060e558f89..21a0fd81798 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -599,6 +599,9 @@ main_sources(COMMON_SRC navigation/navigation_pos_estimator_flow.c navigation/navigation_private.h navigation/navigation_rover_boat.c + navigation/navigation_vtol_mc_protection.c + navigation/navigation_vtol_mc_protection.h + navigation/navigation_vtol_mc_protection_logic.h navigation/navigation_geozone.c navigation/sqrt_controller.c navigation/sqrt_controller.h diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f5225ca3e1e..4414fed501d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -59,6 +59,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" #include "flight/rpm_filter.h" @@ -1366,10 +1367,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 c2cc30a3a48..36ecaf74ba5 100644 --- a/src/main/common/log.c +++ b/src/main/common/log.c @@ -205,9 +205,11 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t { // Print lines of up to maxBytes bytes. We need 5 characters per byte // 0xAB[space|\n] - const size_t charsPerByte = 5; - const size_t maxBytes = 8; - char buf[LOG_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator + enum { + LOG_BUFFER_HEX_CHARS_PER_BYTE = 5, + LOG_BUFFER_HEX_MAX_BYTES = 8, + }; + char buf[LOG_PREFIX_FORMATTED_SIZE + LOG_BUFFER_HEX_CHARS_PER_BYTE * LOG_BUFFER_HEX_MAX_BYTES + 1]; // +1 for the null terminator size_t bufPos = LOG_PREFIX_FORMATTED_SIZE; const uint8_t *inputPtr = buffer; @@ -219,7 +221,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t for (size_t ii = 0; ii < size; ii++) { tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]); - bufPos += charsPerByte; + bufPos += LOG_BUFFER_HEX_CHARS_PER_BYTE; if (bufPos == sizeof(buf)-1) { buf[bufPos-1] = '\n'; buf[bufPos] = '\0'; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 27713297c86..a7852c0758c 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -227,7 +227,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..304766b362c 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, 10); +#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,16 @@ 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, + .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..746a9f6fd48 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -31,6 +31,12 @@ #define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz +typedef enum { + VTOL_MC_PROTECTION_OFF = 0, + VTOL_MC_PROTECTION_NAV, + VTOL_MC_PROTECTION_NAV_AND_STABILIZED, +} vtolMcProtectionMode_e; + typedef enum { FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, @@ -78,6 +84,16 @@ typedef struct systemConfig_s { uint8_t i2c_speed; #endif uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. +#ifdef USE_AUTO_TRANSITION + uint16_t vtolTransitionToFwMinAirspeed; + uint16_t vtolTransitionToMcMaxAirspeed; + uint16_t vtolFwToMcAutoSwitchAirspeed; + uint8_t 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 65654ccd97b..1c01643d5cb 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -445,8 +445,13 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)), BOXMULTIFUNCTION); #endif #if (MAX_MIXER_PROFILE_COUNT > 1) +#ifdef USE_AUTO_TRANSITION + CHECK_ACTIVE_BOX(IS_ENABLED(isMixerProfile2ModeReportedActive()), BOXMIXERPROFILE); + CHECK_ACTIVE_BOX(IS_ENABLED(isMixerTransitionModeReportedActive()), BOXMIXERTRANSITION); +#else CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION); +#endif #endif CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 15a0c0fff4f..4984f89115f 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 @@ -1264,16 +1275,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 trusted pitot airspeed is not being used, INAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, airspeed-linked power and control changes also fall back to this timer whenever trusted pitot is not usable." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 max: 200 + - name: mixer_vtol_transition_dynamic_mixer + description: "Turns on smooth VTOL transition 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 pitot airspeed during an airspeed-controlled transition. This timer does not complete the transition; it only aborts it if the target airspeed is still not reached in time. If pitot becomes unavailable, INAV falls back to `mixer_switch_trans_timer` instead. Set to 0 to disable. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: mixer_config.vtolTransitionAirspeedTimeoutMs + min: 0 + max: 60000 + - name: mixer_vtol_transition_scale_ramp_time_ms + description: "Controls time-based smooth VTOL motor, power, and transition-servo movement. In MC->FW it moves the forward motor from idle to the requested power. After the switch to FW, old lift motors that are no longer used by the FW profile are moved to idle over the same time. In FW->MC it moves the forward motor down to idle while target MC lift power plus target MC motor stabilisation come back in. `INPUT_MIXER_TRANSITION` also uses this time when `mixer_vtol_transition_dynamic_mixer` is ON. If a profile switch or direct switch changes a transition-linked servo output, INAV starts a fresh smooth movement from the servo's current output using this full time value. `0` applies these time-based changes immediately. This timer does not decide when transition completes; completion still uses trusted pitot airspeed or `mixer_switch_trans_timer`. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: mixer_config.vtolTransitionScaleRampTimeMs + 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 @@ -2556,13 +2593,13 @@ groups: field: general.flags.disarm_on_landing type: bool - name: nav_land_detect_sensitivity - description: "Changes sensitivity of landing detection. Higher values increase speed of detection but also increase risk of false detection. Default value should work in most cases." + description: "Scales the generic landing detector velocity and gyro thresholds. `5` is nominal sensitivity: for MC this is about 100 cm/s horizontal and vertical speed plus 4 deg/s average pitch/roll gyro rate. Higher values relax these thresholds and can detect landing sooner, but increase false-detect risk. VTOL MC landing detection also uses additional hard vertical-speed and throttle-probe safety gates that this setting does not bypass." default_value: 5 field: general.land_detect_sensitivity min: 1 max: 15 - name: nav_landing_bump_detection - description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS)." + description: "Allows landing detection to use a touchdown G-bump candidate when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS). For VTOL MC mode the G-bump is not an immediate disarm shortcut: trusted high AGL blocks it, and accepted candidates must still pass the VTOL MC throttle-probe confirmation." default_value: OFF field: general.flags.landing_bump_detection type: bool @@ -2621,6 +2658,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 @@ -3965,6 +4033,61 @@ groups: field: throttle_tilt_compensation_strength min: 0 max: 100 + - name: vtol_transition_to_fw_min_airspeed_cm_s + description: "Minimum pitot airspeed [cm/s] needed before MC->FW transition is considered complete while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this target is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: vtolTransitionToFwMinAirspeed + min: 0 + max: 20000 + - name: vtol_transition_to_mc_max_airspeed_cm_s + description: "When slowing down from FW to MC, the transition is considered complete once pitot airspeed falls to this value [cm/s] or lower while pitot remains usable. If pitot becomes unavailable, or if this is set to 0, INAV uses `mixer_switch_trans_timer` instead. If pitot remains usable but this condition is still not reached before `mixer_vtol_transition_airspeed_timeout_ms` expires, the transition is aborted. Available only on targets with more than 512 KB flash." + 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 trusted pitot airspeed falls to this value or lower while in FW, INAV automatically starts FW->MC. In manual FW flight this requires `mixer_vtol_manualswitch_autotransition_controller` to be ON and keeps MC until the pilot deliberately commands another manual profile change. In mission/RTH/failsafe this requires `mixer_automated_switch` to be ON and keeps the current navigation task in MC after the safety switch, blocking automatic MC->FW re-entry for that navigation session. `vtol_transition_to_mc_max_airspeed_cm_s` still controls when the FW->MC transition is safe to complete. Set to 0 to disable. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 0 + field: vtolFwToMcAutoSwitchAirspeed + min: 0 + max: 20000 + - name: vtol_transition_lift_min_percent + description: "Lowest lift motor power used during transition, in percent. In MC->FW, lift power is reduced to this value as the transition progresses. In FW->MC, lift power starts from this value and rises back to full power by the motor ramp timer. `100` keeps full lift power through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 100 + field: vtolTransitionLiftMinPercent + min: 0 + max: 100 + - name: vtol_transition_mc_authority_min_percent + description: "Lowest multicopter stabilisation used during transition, in percent. In MC->FW, active MC motor stabilisation is reduced to this value as the transition progresses. In FW->MC, target MC motor stabilisation starts from this value and rises back to full strength by the motor ramp timer. `100` keeps full multicopter stabilisation through the whole transition. Used only when `mixer_vtol_transition_dynamic_mixer` is ON. Available only on targets with more than 512 KB flash." + condition: USE_AUTO_TRANSITION + default_value: 100 + field: vtolTransitionMcAuthorityMinPercent + min: 0 + max: 100 + - name: vtol_transition_fw_authority_min_percent + description: "Lowest fixed-wing stabilisation used during transition, in percent. In MC->FW, fixed-wing stabilisation starts from this value and rises to full strength as the 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 a80992b772d..4da18bc8ccf 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -48,6 +48,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" @@ -264,6 +265,53 @@ void mixerResetDisarmedMotors(void) motor_disarmed[i] = motorZeroCommand; } } + +#ifdef USE_AUTO_TRANSITION +typedef struct autoTransitionMotorMixState_s { + bool active; + bool currentProfileIsMultirotor; + bool targetProfileIsMultirotor; + const motorMixer_t *targetMotorMixer; + const mixerConfig_t *targetMixerConfig; + int16_t targetInput[3]; +} autoTransitionMotorMixState_t; + +static void NOINLINE prepareAutoTransitionMotorMixState(autoTransitionMotorMixState_t *state) +{ + if (!(isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction != MIXERAT_DIRECTION_NONE && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT)) { + return; + } + + state->active = true; + state->currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); + state->targetMotorMixer = mixerMotorMixersByIndex(nextMixerProfileIndex); + state->targetMixerConfig = mixerConfigByIndex(nextMixerProfileIndex); + state->targetProfileIsMultirotor = isMultirotorTypePlatform(state->targetMixerConfig->platformType); + + const float targetAuthorityScale = state->targetProfileIsMultirotor ? + mixerATGetMcAuthorityScale() : + mixerATGetFwAuthorityScale(); + + if (!state->targetProfileIsMultirotor && FLIGHT_MODE(MANUAL_MODE)) { + state->targetInput[ROLL] = rcCommand[ROLL]; + state->targetInput[PITCH] = rcCommand[PITCH]; + state->targetInput[YAW] = rcCommand[YAW]; + } else { + state->targetInput[ROLL] = getAutoTransitionTargetAxisPID(FD_ROLL); + state->targetInput[PITCH] = getAutoTransitionTargetAxisPID(FD_PITCH); + state->targetInput[YAW] = getAutoTransitionTargetAxisPID(FD_YAW); + } + + state->targetInput[ROLL] = state->targetInput[ROLL] * (state->targetMixerConfig->transition_PID_mmix_multiplier_roll / 1000.0f) * targetAuthorityScale; + state->targetInput[PITCH] = state->targetInput[PITCH] * (state->targetMixerConfig->transition_PID_mmix_multiplier_pitch / 1000.0f) * targetAuthorityScale; + state->targetInput[YAW] = state->targetInput[YAW] * (state->targetMixerConfig->transition_PID_mmix_multiplier_yaw / 1000.0f) * targetAuthorityScale; +} +#endif + #if !defined(SITL_BUILD) static uint16_t handleOutputScaling( int16_t input, // Input value from the mixer @@ -520,13 +568,30 @@ 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); + } +#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 +599,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 +722,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 +769,48 @@ 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(); + motor[i] = constrain(rpyMix[i] + constrain(targetMotorThrottle, throttleMin, throttleMax), throttleRangeMin, throttleRangeMax); + } +#endif //spin stopped motors only in mixer transition mode if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && !feature(FEATURE_REVERSIBLE_MOTORS)) { +#ifdef USE_AUTO_TRANSITION + 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 } } @@ -737,4 +906,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 6c4370d4176..7b48d148884 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..63735c4b799 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -10,13 +10,19 @@ #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 "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 +35,11 @@ #include "navigation/navigation.h" #include "common/log.h" +#include "build/debug.h" + +#ifdef USE_AUTO_TRANSITION +#define MIXER_TRANSITION_OSD_EVENT_DISPLAY_MS 3000 +#endif mixerConfig_t currentMixerConfig; int currentMixerProfileIndex; @@ -36,8 +47,25 @@ 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; +#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 +81,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, @@ -94,24 +128,666 @@ void mixerConfigInit(void) servosInit(); mixerUpdateStateFlags(); mixerInit(); - if (currentMixerConfig.controlProfileLinking) - { - // LOG_INFO(PWM, "mixer switch pidInit"); + + if (currentMixerConfig.controlProfileLinking) { setConfigProfile(getConfigMixerProfile()); - pidInit(); - pidInitFilters(); - pidResetErrorAccumulators(); //should be safer to reset error accumulators - schedulePidGainsUpdate(); - navigationUsePIDs(); // set navigation pid gains } + + // Reinitialize the active controller on every mixer hot-switch so AUTO PID + // selection follows the new platform type and no stale FW/MC integrator + // state leaks across the completed transition. + pidInit(); + pidInitFilters(); + pidResetErrorAccumulators(); + schedulePidGainsUpdate(); + navigationUsePIDs(); } void setMixerProfileAT(void) { +#ifdef USE_AUTO_TRANSITION + const timeMs_t now = millis(); + + mixerProfileAT.transitionStartTime = now; + mixerProfileAT.aborted = false; + mixerProfileAT.abortedByAirspeedTimeout = false; + mixerProfileAT.hotSwitchDone = false; + mixerProfileAT.usedAirspeed = false; + mixerProfileAT.transitionStartAirspeedCaptured = false; + mixerProfileAT.progress = 0.0f; + mixerProfileAT.handoffScalingProgress = 0.0f; + mixerProfileAT.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.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); + mixerProfileAT.servoHandoffMask = 0; + mixerProfileAT.servoHandoffDurationMs = 0; + mixerProfileAT.servoHandoffHoldDurationMs = 0; + mixerProfileAT.servoHandoffStartTime = 0; + mixerProfileAT.servoHandoffHoldStartTime = 0; + memset(mixerProfileAT.servoHandoffOutput, 0, sizeof(mixerProfileAT.servoHandoffOutput)); +#else mixerProfileAT.transitionStartTime = millis(); mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; +#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 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 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) +{ + clearServoHandoffFade(); + const bool switched = outputProfileHotSwitch(profileIndex); + clearServoHandoffFade(); + return switched; +} + +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.progress = 0.0f; + mixerProfileAT.handoffScalingProgress = 0.0f; + 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.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); +} + +static void setLegacyTransitionScales(void) +{ + mixerProfileAT.progress = 1.0f; + mixerProfileAT.handoffScalingProgress = 1.0f; + mixerProfileAT.motorRampProgress = 1.0f; + mixerProfileAT.blendToFw = 1.0f; + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; + mixerProfileAT.postSwitchFadeProgress = 1.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); +} + +static 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 getHandoffScalingProgress(void) +{ + mixerProfileAT.handoffScalingProgress = mixerTransitionResolveHandoffProgress( + currentMixerConfig.vtolTransitionDynamicMixer, + mixerProfileAT.usedAirspeed, + mixerProfileAT.handoffScalingProgress, + mixerProfileAT.progress); + return mixerProfileAT.handoffScalingProgress; +} + +static bool hasTrustedPitotAirspeed(float *airspeedCmS) +{ +#ifdef USE_PITOT + if (!sensors(SENSOR_PITOT) || !pitotGetValidForAirspeed() || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE || + detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + return false; + } + + *airspeedCmS = pitot.airSpeed; + return true; +#else + UNUSED(airspeedCmS); + return false; +#endif +} + +static bool hasPitotSensorForManualProtection(void) +{ +#ifdef USE_PITOT + if (!sensors(SENSOR_PITOT) || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE || + detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + return false; + } + + return true; +#else + return false; +#endif +} + +static void preparePostSwitchFade(const int targetProfileIndex) +{ + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.postSwitchFadeInitialScale = 0.0f; + mixerProfileAT.postSwitchFadeDurationMs = 0; + mixerProfileAT.postSwitchFadeStartTime = 0; + memset(mixerProfileAT.postSwitchFadeMotorOutput, 0, sizeof(mixerProfileAT.postSwitchFadeMotorOutput)); + + if (!currentMixerConfig.vtolTransitionDynamicMixer || + currentMixerConfig.vtolTransitionScaleRampTimeMs == 0 || + mixerProfileAT.direction == MIXERAT_DIRECTION_NONE || + targetProfileIndex < 0 || + targetProfileIndex >= MAX_MIXER_PROFILE_COUNT) { + return; + } + + const motorMixer_t *currentMotorMixer = mixerMotorMixersByIndex(currentMixerProfileIndex); + const motorMixer_t *targetMotorMixer = mixerMotorMixersByIndex(targetProfileIndex); + const bool currentProfileIsMultirotor = isMultirotorTypePlatform(currentMixerConfig.platformType); + const uint8_t count = getMotorCount(); + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + currentMixerConfig.vtolTransitionDynamicMixer, + currentMixerConfig.vtolTransitionScaleRampTimeMs, + mixerProfileAT.direction, + currentProfileIsMultirotor, + count, + currentMotorMixer, + targetMotorMixer); + + mixerProfileAT.postSwitchFadeMotorMask = fadeMask.motorMask; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = fadeMask.toCurrentMotorMask; + + for (uint8_t i = 0; i < count && i < MAX_SUPPORTED_MOTORS; i++) { + if (mixerProfileAT.postSwitchFadeMotorMask & (1U << i)) { + mixerProfileAT.postSwitchFadeMotorOutput[i] = motor[i]; + } + } + + if (mixerProfileAT.postSwitchFadeMotorMask == 0) { + return; + } + + mixerProfileAT.postSwitchFadeDurationMs = currentMixerConfig.vtolTransitionScaleRampTimeMs; + mixerProfileAT.postSwitchFadeInitialScale = mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW ? + constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) : + constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f); +} + +static bool startPostSwitchFade(void) +{ + if (mixerProfileAT.postSwitchFadeMotorMask == 0 || + mixerProfileAT.postSwitchFadeDurationMs == 0) { + return false; + } + + mixerProfileAT.postSwitchFadeStartTime = millis(); + mixerProfileAT.postSwitchFadeProgress = 0.0f; + mixerProfileAT.phase = MIXERAT_PHASE_POST_SWITCH_FADE; + isMixerTransitionMixing_requested = false; + return true; +} + +static bool updatePostSwitchFade(void) +{ + if (mixerProfileAT.phase != MIXERAT_PHASE_POST_SWITCH_FADE) { + return true; + } + + if (mixerProfileAT.postSwitchFadeDurationMs == 0) { + mixerProfileAT.postSwitchFadeProgress = 1.0f; + } else { + const uint32_t elapsedMs = millis() - mixerProfileAT.postSwitchFadeStartTime; + mixerProfileAT.postSwitchFadeProgress = constrainf((float)elapsedMs / (float)mixerProfileAT.postSwitchFadeDurationMs, 0.0f, 1.0f); + } + + const float remainingScale = mixerProfileAT.postSwitchFadeInitialScale * (1.0f - mixerProfileAT.postSwitchFadeProgress); + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = remainingScale; + mixerProfileAT.mcAuthorityScale = 0.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; + mixerProfileAT.blendToFw = 1.0f; + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + mixerProfileAT.pusherScale = remainingScale; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 0.0f; + mixerProfileAT.blendToFw = 0.0f; + } + + if (mixerProfileAT.postSwitchFadeProgress < 1.0f) { + return false; + } + + setMixerATOsdEvent(MIXERAT_OSD_EVENT_DONE); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + mixerProfileAT.postSwitchFadeMotorMask = 0; + mixerProfileAT.postSwitchFadeToCurrentMotorMask = 0; + return true; +} + +static uint16_t getAirspeedThresholdForDirection(const mixerProfileATDirection_e direction) +{ + if (direction == MIXERAT_DIRECTION_TO_FW) { + return systemConfig()->vtolTransitionToFwMinAirspeed; + } + + if (direction == MIXERAT_DIRECTION_TO_MC) { + return systemConfig()->vtolTransitionToMcMaxAirspeed; + } + + return 0; +} + +static bool shouldRequestManualFwToMcProtection(const bool manualControllerEnabled) +{ + if (!manualControllerEnabled || !STATE(AIRPLANE)) { + return false; + } + + const uint16_t thresholdCmS = systemConfig()->vtolFwToMcAutoSwitchAirspeed; + if (thresholdCmS == 0 || !hasPitotSensorForManualProtection()) { + return false; + } + + float airspeedCmS = 0.0f; + if (!hasTrustedPitotAirspeed(&airspeedCmS)) { + return false; + } + + return 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, + getHandoffScalingProgress(), + getMotorRampProgress()); + + mixerProfileAT.blendToFw = scales.blendToFw; + mixerProfileAT.pusherScale = scales.pusherScale; + mixerProfileAT.liftScale = scales.liftScale; + mixerProfileAT.mcAuthorityScale = scales.mcAuthorityScale; + mixerProfileAT.fwAuthorityScale = scales.fwAuthorityScale; +} + +static void abortTransition(const bool byAirspeedTimeout, 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 : MIXERAT_OSD_EVENT_ABORTED); + } + + isMixerTransitionMixing_requested = false; + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.aborted = wasActive; + mixerProfileAT.abortedByAirspeedTimeout = wasActive && byAirspeedTimeout; + mixerProfileAT.hotSwitchDone = false; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + mixerProfileAT.usedAirspeed = false; + mixerProfileAT.transitionStartAirspeedCaptured = false; + mixerProfileAT.transitionStartAirspeedCmS = 0.0f; + resetTransitionScales(); + + 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 uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + const uint32_t transitionTimerMs = MAX(0, currentMixerConfig.switchTransitionTimer) * 100; + float airspeedCmS = 0.0f; + const bool trustedAirspeedAvailable = + airspeedThresholdCmS > 0 && hasTrustedPitotAirspeed(&airspeedCmS); + const mixerTransitionHotSwitchProgress_t hotSwitchProgress = mixerTransitionEvaluateHotSwitch( + direction, + airspeedThresholdCmS, + trustedAirspeedAvailable, + airspeedCmS, + mixerProfileAT.transitionStartAirspeedCaptured, + mixerProfileAT.transitionStartAirspeedCmS, + elapsedMs, + transitionTimerMs); + + mixerProfileAT.usedAirspeed = hotSwitchProgress.usedAirspeed; + mixerProfileAT.transitionStartAirspeedCaptured = hotSwitchProgress.transitionStartAirspeedCaptured; + mixerProfileAT.transitionStartAirspeedCmS = hotSwitchProgress.transitionStartAirspeedCmS; + mixerProfileAT.progress = hotSwitchProgress.progress; + return hotSwitchProgress.readyForHotSwitch; +} +#endif + bool platformTypeConfigured(flyingPlatformType_e platformType) { if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){ @@ -120,18 +796,38 @@ bool platformTypeConfigured(flyingPlatformType_e platformType) return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType; } -bool checkMixerATRequired(mixerProfileATRequest_e required_action) +#ifdef USE_AUTO_TRANSITION +static bool missionTransitionToMultirotorTypeConfigured(void) { - //return false if mixerAT condition is not required or setting is not valid - if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) - { - return false; - } - if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) - { + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) { return false; } + const flyingPlatformType_e nextPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; + return isMultirotorTypePlatform(nextPlatformType); +} +#endif + +#ifdef USE_AUTO_TRANSITION +static bool isLegacyManualTransitionSessionActive(void) +{ + return manualTransitionSessionMode == MIXER_TRANSITION_MANUAL_SESSION_LEGACY; +} +#endif + +bool checkMixerATRequired(mixerProfileATRequest_e required_action) +{ +#ifdef USE_AUTO_TRANSITION + return mixerTransitionIsRequestAllowed( + required_action, + STATE(AIRPLANE), + STATE(MULTIROTOR), + isModeActivationConditionPresent(BOXMIXERPROFILE), + currentMixerConfig.automated_switch, + platformTypeConfigured(PLATFORM_AIRPLANE), + missionTransitionToMultirotorTypeConfigured()); +#else + // Legacy 512 KB targets keep the original automated-switch behaviour. if(currentMixerConfig.automated_switch){ if ((required_action == MIXERAT_REQUEST_RTH) && STATE(MULTIROTOR)) { @@ -143,14 +839,100 @@ 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(); + 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)) { + 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 (!startPostSwitchFade()) { + setMixerATOsdEvent(MIXERAT_OSD_EVENT_DONE); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + } + return true; + } else if (mixerProfileAT.usedAirspeed && + currentMixerConfig.vtolTransitionAirspeedTimeoutMs > 0 && + (millis() - mixerProfileAT.transitionStartTime) >= currentMixerConfig.vtolTransitionAirspeedTimeoutMs) { + abortTransition(true, false); + return true; + } + + updateTransitionScales(); + return false; + break; + case MIXERAT_PHASE_POST_SWITCH_FADE: + isMixerTransitionMixing_requested = false; + updatePostSwitchFade(); + 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 +948,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 +969,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) } while (reprocessState); return true; +#endif } bool checkMixerProfileHotSwitchAvalibility(void) @@ -202,7 +984,274 @@ 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 manualControllerConfigured = currentMixerConfig.manualVtolTransitionController && !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); + + manualTransitionSessionMode = mixerTransitionUpdateManualSessionMode( + manualTransitionSessionMode, + transitionModeRisingEdge, + transitionModeFallingEdge && !keepCompletedAutoSession, + manualControllerConfigured, + false); + + if (transitionModeRisingEdge) { + manualFwToMcProtectionLatched = false; + } + + if (!transitionModeActive && + manualTransitionSessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && + mixerProfileAT.hotSwitchDone && + requestedProfileIndex == currentMixerProfileIndex) { + manualTransitionSessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + } + + if (requestedMultirotorProfile || (!mixerAT_inuse && !STATE(MULTIROTOR))) { + manualFwToMcProtectionLatched = false; + } + + 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, + transitionModeRisingEdge, + transitionModeActive, + currentMixerProfileIndex, + requestedProfileIndex)) { + navigationProfileHandbackPending = false; + } else if (mixerTransitionNavigationHandbackShouldHoldProfile( + navigationProfileSwitchWasOwned, + navigationOwnsProfileSwitch, + mixerProfileModePresent, + mixerAT_inuse, + transitionModeActive, + currentMixerProfileIndex, + requestedProfileIndex)) { + navigationProfileHandbackPending = true; + } + navigationProfileSwitchWasOwned = navigationOwnsProfileSwitch; + + const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; + const bool completedAutoSessionOwnsProfileSwitch = mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + manualTransitionSessionMode, + mixerProfileAT.hotSwitchDone, + currentMixerProfileIndex, + requestedProfileIndex); + + if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) + { + if (mixerProfileModePresent && + !transitionControllerOwnsProfileSwitch && + !completedAutoSessionOwnsProfileSwitch && + !navigationOwnsProfileSwitch && + !navigationProfileHandbackPending && + !fwToMcProtectionOwnsProfileSwitch) { + if (requestedProfileIndex != currentMixerProfileIndex) { + outputProfileDirectSwitch(requestedProfileIndex); + } + } + } + + // Recompute after a potential direct profile hot-switch because this flag is per-mixer-profile. + manualControllerEnabled = mixerTransitionManualControllerEnabled( + currentMixerConfig.manualVtolTransitionController && !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 (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) { + 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 && manualTransitionReadyForEdge && manualTransitionAllowed && !mixerAT_inuse) { + manualTransitionReadyForEdge = false; + if (STATE(MULTIROTOR)) { + mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_FW); + } else if (STATE(AIRPLANE)) { + mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_MC); + } + mixerAT_inuse = mixerATIsActive(); + } + + if (!transitionModeActive && + mixerAT_inuse && + !mixerProfileAT.hotSwitchDone && + (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { + abortTransition(false, true); + mixerAT_inuse = false; + if (!FLIGHT_MODE(FAILSAFE_MODE) && + mixerProfileModePresent && + !navigationOwnsProfileSwitch && + requestedProfileIndex != currentMixerProfileIndex) { + outputProfileDirectSwitch(requestedProfileIndex); + } + } + + if (mixerAT_inuse && + (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { + mixerATUpdateState(mixerProfileAT.request); + 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) + // [2] packed transition flags | active platform type | active PID type | target input mode + // [3] raw transition progress x1000 + // [4] pusherScale x1000 + // [5] liftScale x1000 + // [6] mcAuthorityScale x1000 | (fwAuthorityScale x1000 << 16) + // [7] handoffProgress 10-bit | motorRampProgress 10-bit | postSwitchFadeProgress 10-bit + DEBUG_SET(DEBUG_VTOL_TRANSITION, 0, mixerProfileAT.phase); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, (int32_t)(((uint32_t)mixerProfileAT.request & 0xFFU) | (((uint32_t)mixerProfileAT.direction & 0xFFU) << 8))); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 2, (int32_t)transitionDebugFlags); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 3, progressScaled); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 4, pusherScaled); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 5, liftScaled); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 6, (int32_t)((uint32_t)mcAuthorityScaled | ((uint32_t)fwAuthorityScaled << 16))); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, (int32_t)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 +1262,216 @@ 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->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 float progress = mixerProfileAT.servoHandoffDurationMs == 0 ? + 1.0f : + constrainf((float)(millis() - mixerProfileAT.servoHandoffStartTime) / (float)mixerProfileAT.servoHandoffDurationMs, 0.0f, 1.0f); + + *output = mixerTransitionBlendCapturedServoOutput( + mixerProfileAT.servoHandoffOutput[servoIndex], + currentOutput, + progress); + + if (progress >= 1.0f) { + clearServoHandoffFade(); + } + + return true; +} + +bool 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; + + *output = mixerTransitionBlendCapturedMotorOutput( + capturedOutput, + targetOutput, + mixerProfileAT.postSwitchFadeProgress); + return true; +} + +float mixerATGetPostSwitchFadeProgress(void) +{ + return constrainf(mixerProfileAT.postSwitchFadeProgress, 0.0f, 1.0f); +} +#endif + +bool isMixerProfile2ModeReportedActive(void) +{ +#if (MAX_MIXER_PROFILE_COUNT > 1) + return currentMixerProfileIndex > 0; +#else + return false; +#endif +} + +bool isMixerTransitionModeReportedActive(void) +{ +#ifdef USE_AUTO_TRANSITION + // Transition is actively running in the internal controller. + if (mixerATIsActive()) { + return true; + } + + // With manual auto-transition enabled (or session latched), treat RC as trigger only. + if (mixerTransitionManualControllerEnabled(currentMixerConfig.manualVtolTransitionController, manualTransitionSessionMode)) { + return false; + } + + return IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); +#else + return IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); +#endif } // switch mixerprofile without reboot diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 715732d0685..73046913b5e 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,116 @@ 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 +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_AIRSPEED_TIMEOUT, +} mixerProfileATOsdEvent_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, +#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 abortedByAirspeedTimeout; + bool hotSwitchDone; + bool usedAirspeed; + bool transitionStartAirspeedCaptured; + float progress; + float handoffScalingProgress; + 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 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 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; + 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); +#endif +bool isMixerProfile2ModeReportedActive(void); +bool isMixerTransitionModeReportedActive(void); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; @@ -81,4 +176,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..5d7ff20a8c3 --- /dev/null +++ b/src/main/flight/mixer_transition_logic.h @@ -0,0 +1,590 @@ +#pragma once + +#include +#include + +#include "flight/mixer_profile.h" + +typedef enum { + MIXER_TRANSITION_MANUAL_SESSION_NONE = 0, + MIXER_TRANSITION_MANUAL_SESSION_LEGACY, + MIXER_TRANSITION_MANUAL_SESSION_AUTO, +} mixerTransitionManualSessionMode_e; + +#ifdef USE_AUTO_TRANSITION +typedef struct { + bool readyForHotSwitch; + bool usedAirspeed; + bool transitionStartAirspeedCaptured; + float transitionStartAirspeedCmS; + float progress; +} mixerTransitionHotSwitchProgress_t; + +typedef struct { + float pusherScale; + float liftScale; + float mcAuthorityScale; + float fwAuthorityScale; + float blendToFw; +} mixerTransitionScaleState_t; + +typedef struct { + uint16_t motorMask; + uint16_t toCurrentMotorMask; +} mixerTransitionPostSwitchFadeMask_t; +#endif + +static inline mixerTransitionManualSessionMode_e mixerTransitionUpdateManualSessionMode( + mixerTransitionManualSessionMode_e currentMode, + bool transitionModeRisingEdge, + bool transitionModeFallingEdge, + bool manualControllerConfigured, + bool clearSession) +{ + if (clearSession || transitionModeFallingEdge) { + return MIXER_TRANSITION_MANUAL_SESSION_NONE; + } + + if (transitionModeRisingEdge) { + return manualControllerConfigured ? + MIXER_TRANSITION_MANUAL_SESSION_AUTO : + MIXER_TRANSITION_MANUAL_SESSION_LEGACY; + } + + return currentMode; +} + +static inline float mixerTransitionClamp(float value, float low, float high) +{ + if (value < low) { + return low; + } + + if (value > high) { + return high; + } + + return value; +} + +static inline bool mixerTransitionManualControllerEnabled( + bool manualControllerConfigured, + mixerTransitionManualSessionMode_e sessionMode) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO || + (sessionMode != MIXER_TRANSITION_MANUAL_SESSION_LEGACY && manualControllerConfigured); +} + +#ifdef USE_AUTO_TRANSITION +static inline bool mixerTransitionKeepCompletedAutoSession( + mixerTransitionManualSessionMode_e sessionMode, + bool transitionModeFallingEdge, + bool hotSwitchDone, + int currentProfileIndex, + int requestedProfileIndex) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && + !transitionModeFallingEdge && + hotSwitchDone && + currentProfileIndex != requestedProfileIndex; +} + +static inline bool mixerTransitionCompletedAutoSessionOwnsProfileSwitch( + mixerTransitionManualSessionMode_e sessionMode, + bool hotSwitchDone, + int currentProfileIndex, + int requestedProfileIndex) +{ + return sessionMode == MIXER_TRANSITION_MANUAL_SESSION_AUTO && + hotSwitchDone && + currentProfileIndex != requestedProfileIndex; +} + +static inline mixerProfileATDirection_e mixerTransitionManualSwitchReminderDirection( + mixerTransitionManualSessionMode_e sessionMode, + bool autoTransitionActive, + bool hotSwitchDone, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex, + bool currentProfileIsMultirotor) +{ + if (sessionMode != MIXER_TRANSITION_MANUAL_SESSION_AUTO || + autoTransitionActive || + !hotSwitchDone) { + return MIXERAT_DIRECTION_NONE; + } + + if (!transitionModeActive && currentProfileIndex == requestedProfileIndex) { + return MIXERAT_DIRECTION_NONE; + } + + return currentProfileIsMultirotor ? MIXERAT_DIRECTION_TO_MC : MIXERAT_DIRECTION_TO_FW; +} + +static inline bool mixerTransitionNavigationHandbackShouldHoldProfile( + bool navigationOwnedProfileSwitchPreviousUpdate, + bool navigationOwnsProfileSwitch, + bool mixerProfileModePresent, + bool autoTransitionActive, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex) +{ + return navigationOwnedProfileSwitchPreviousUpdate && + !navigationOwnsProfileSwitch && + mixerProfileModePresent && + !autoTransitionActive && + (transitionModeActive || currentProfileIndex != requestedProfileIndex); +} + +static inline bool mixerTransitionNavigationHandbackShouldClear( + bool navigationOwnsProfileSwitch, + bool transitionModeRisingEdge, + bool transitionModeActive, + int currentProfileIndex, + int requestedProfileIndex) +{ + return navigationOwnsProfileSwitch || + transitionModeRisingEdge || + (!transitionModeActive && currentProfileIndex == requestedProfileIndex); +} + +static inline bool mixerTransitionShouldClearCompletedAutoMixingRequest( + bool transitionModeActive, + bool transitionModeRisingEdge, + bool autoTransitionActive, + bool hotSwitchDone) +{ + return transitionModeActive && !transitionModeRisingEdge && !autoTransitionActive && hotSwitchDone; +} + +static inline bool mixerTransitionNavigationOwnsProfileSwitch( + bool armed, + bool vtolProfilePairConfigured, + bool navWaypointActive, + bool navRthActive, + bool navLandingActive, + bool navMixerAtActive) +{ + return armed && + vtolProfilePairConfigured && + (navWaypointActive || navRthActive || navLandingActive || navMixerAtActive); +} +#endif + +static inline bool mixerTransitionIsRequestAllowed( + mixerProfileATRequest_e requiredAction, + bool stateAirplane, + 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 int16_t mixerTransitionRoundFloatToInt16(float value) +{ + return (int16_t)(value >= 0.0f ? value + 0.5f : value - 0.5f); +} + +static inline float mixerTransitionResolveHandoffProgress( + bool dynamicMixerEnabled, + bool usedAirspeed, + float previousHandoffProgress, + float rawProgress) +{ + const float clampedProgress = mixerTransitionClamp(rawProgress, 0.0f, 1.0f); + + if (!dynamicMixerEnabled) { + return 1.0f; + } + + if (usedAirspeed) { + return clampedProgress; + } + + return previousHandoffProgress > clampedProgress ? previousHandoffProgress : clampedProgress; +} + +static inline float mixerTransitionBlendScale(float from, float to, float progress) +{ + return from + (to - from) * mixerTransitionClamp(progress, 0.0f, 1.0f); +} + +static inline int16_t mixerTransitionBlendCapturedMotorOutput( + int16_t capturedOutput, + int16_t targetOutput, + float progress) +{ + const float holdScale = 1.0f - mixerTransitionClamp(progress, 0.0f, 1.0f); + const int32_t blendedOutput = mixerTransitionRoundFloatToInt16( + targetOutput + (capturedOutput - targetOutput) * holdScale); + const int16_t low = targetOutput < capturedOutput ? targetOutput : capturedOutput; + const int16_t high = targetOutput > capturedOutput ? targetOutput : capturedOutput; + + if (blendedOutput < low) { + return low; + } + + if (blendedOutput > high) { + return high; + } + + return (int16_t)blendedOutput; +} + +static inline mixerTransitionScaleState_t mixerTransitionComputeScales( + bool dynamicMixerEnabled, + mixerProfileATDirection_e direction, + float liftFloor, + float mcFloor, + float fwFloor, + float handoffProgress, + float motorRampProgress) +{ + mixerTransitionScaleState_t scales = { + .pusherScale = 1.0f, + .liftScale = 1.0f, + .mcAuthorityScale = 1.0f, + .fwAuthorityScale = 1.0f, + .blendToFw = 1.0f, + }; + + if (!dynamicMixerEnabled) { + return scales; + } + + liftFloor = mixerTransitionClamp(liftFloor, 0.0f, 1.0f); + mcFloor = mixerTransitionClamp(mcFloor, 0.0f, 1.0f); + fwFloor = mixerTransitionClamp(fwFloor, 0.0f, 1.0f); + + if (direction == MIXERAT_DIRECTION_TO_FW) { + scales.pusherScale = mixerTransitionBlendScale(0.0f, 1.0f, motorRampProgress); + scales.liftScale = mixerTransitionBlendScale(1.0f, liftFloor, handoffProgress); + scales.mcAuthorityScale = mixerTransitionBlendScale(1.0f, mcFloor, handoffProgress); + scales.fwAuthorityScale = mixerTransitionBlendScale(fwFloor, 1.0f, handoffProgress); + } else if (direction == MIXERAT_DIRECTION_TO_MC) { + scales.pusherScale = mixerTransitionBlendScale(1.0f, 0.0f, motorRampProgress); + scales.liftScale = mixerTransitionBlendScale(liftFloor, 1.0f, motorRampProgress); + scales.mcAuthorityScale = mixerTransitionBlendScale(mcFloor, 1.0f, motorRampProgress); + scales.fwAuthorityScale = mixerTransitionBlendScale(1.0f, fwFloor, handoffProgress); + } + + scales.blendToFw = mixerTransitionClamp(scales.fwAuthorityScale, 0.0f, 1.0f); + return scales; +} + +static inline mixerTransitionHotSwitchProgress_t mixerTransitionEvaluateHotSwitch( + mixerProfileATDirection_e direction, + uint16_t airspeedThresholdCmS, + bool trustedAirspeedAvailable, + float currentAirspeedCmS, + bool transitionStartAirspeedCaptured, + float transitionStartAirspeedCmS, + uint32_t elapsedMs, + uint32_t transitionTimerMs) +{ + mixerTransitionHotSwitchProgress_t result = { + .readyForHotSwitch = false, + .usedAirspeed = false, + .transitionStartAirspeedCaptured = transitionStartAirspeedCaptured, + .transitionStartAirspeedCmS = transitionStartAirspeedCmS, + .progress = 0.0f, + }; + + if (direction == MIXERAT_DIRECTION_NONE) { + return result; + } + + if (airspeedThresholdCmS > 0 && trustedAirspeedAvailable) { + result.usedAirspeed = true; + + if (direction == MIXERAT_DIRECTION_TO_FW) { + result.progress = mixerTransitionClamp(currentAirspeedCmS / airspeedThresholdCmS, 0.0f, 1.0f); + result.readyForHotSwitch = currentAirspeedCmS >= airspeedThresholdCmS; + return result; + } + + if (!result.transitionStartAirspeedCaptured) { + result.transitionStartAirspeedCmS = currentAirspeedCmS; + result.transitionStartAirspeedCaptured = true; + } + + if (result.transitionStartAirspeedCmS <= airspeedThresholdCmS) { + result.progress = 1.0f; + } else { + result.progress = mixerTransitionClamp( + (result.transitionStartAirspeedCmS - currentAirspeedCmS) / + (result.transitionStartAirspeedCmS - airspeedThresholdCmS), + 0.0f, + 1.0f); + } + + result.readyForHotSwitch = currentAirspeedCmS <= airspeedThresholdCmS; + return result; + } + + result.progress = transitionTimerMs > 0 ? + mixerTransitionClamp((float)elapsedMs / (float)transitionTimerMs, 0.0f, 1.0f) : + 1.0f; + result.readyForHotSwitch = elapsedMs >= transitionTimerMs; + return result; +} + +static inline mixerTransitionPostSwitchFadeMask_t mixerTransitionComputePostSwitchFadeMask( + bool dynamicMixerEnabled, + uint16_t scaleRampTimeMs, + mixerProfileATDirection_e direction, + bool currentProfileIsMultirotor, + uint8_t motorCount, + const motorMixer_t *currentMotorMixer, + const motorMixer_t *targetMotorMixer) +{ + mixerTransitionPostSwitchFadeMask_t mask = { 0, 0 }; + + if (!dynamicMixerEnabled || + scaleRampTimeMs == 0 || + direction == MIXERAT_DIRECTION_NONE || + !currentMotorMixer || + !targetMotorMixer) { + return mask; + } + + for (uint8_t i = 0; i < motorCount && i < MAX_SUPPORTED_MOTORS; i++) { + const bool currentMotorActive = currentMotorMixer[i].throttle > 0.0f; + const bool targetMotorActive = targetMotorMixer[i].throttle > 0.0f; + const bool oldLiftMotor = direction == MIXERAT_DIRECTION_TO_FW && + currentProfileIsMultirotor && + currentMotorActive && + !targetMotorActive; + const bool oldPusherMotor = direction == MIXERAT_DIRECTION_TO_MC && + !currentProfileIsMultirotor && + currentMotorActive && + !targetMotorActive; + const bool targetFwPusherMotor = direction == MIXERAT_DIRECTION_TO_FW && + currentProfileIsMultirotor && + !currentMotorActive && + targetMotorActive; + + if (oldLiftMotor || oldPusherMotor || targetFwPusherMotor) { + mask.motorMask |= (1U << i); + if (targetFwPusherMotor) { + mask.toCurrentMotorMask |= (1U << i); + } + } + } + + return mask; +} +#endif + +#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 a8168fa6081..57755b1928b 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,6 +230,11 @@ static EXTENDED_FASTRAM timeMs_t pidLoopNowMs; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; +static void applyItermLimiting(pidState_t *pidState); +static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination); +float pidRcCommandToRate(int16_t stick, uint8_t rate); +int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis); + PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, @@ -416,6 +467,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 } } @@ -435,6 +491,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); @@ -1251,6 +2045,9 @@ void FAST_CODE pidController(float dT) pidLoopNowMs = millis(); if (!pidFiltersConfigured) { +#ifdef USE_AUTO_TRANSITION + resetAutoTransitionTargetPidState(); +#endif return; } @@ -1351,6 +2148,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) @@ -1450,6 +2251,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 68f09018763..71049667f68 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -188,7 +188,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); @@ -228,4 +235,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..64f4166bc69 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -51,6 +51,7 @@ #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" @@ -280,6 +281,79 @@ static void filterServos(void) } } +#ifdef USE_AUTO_TRANSITION +static bool isAutoTransitionTargetInputSource(const uint8_t inputSource) +{ + return inputSource >= INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL && + inputSource <= INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS; +} + +static bool isStabilizedAxisInputSource(const uint8_t inputSource) +{ + switch (inputSource) { + case INPUT_STABILIZED_ROLL: + case INPUT_STABILIZED_PITCH: + case INPUT_STABILIZED_YAW: + case INPUT_STABILIZED_ROLL_PLUS: + case INPUT_STABILIZED_ROLL_MINUS: + case INPUT_STABILIZED_PITCH_PLUS: + case INPUT_STABILIZED_PITCH_MINUS: + case INPUT_STABILIZED_YAW_PLUS: + case INPUT_STABILIZED_YAW_MINUS: + return true; + default: + return false; + } +} + +static int getAutoTransitionServoProfileIndex(void) +{ + if (isMultirotorTypePlatform(currentMixerConfig.platformType)) { + return currentMixerProfileIndex; + } + + if (nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_MIXER_PROFILE_COUNT && + isMultirotorTypePlatform(mixerConfigByIndex(nextMixerProfileIndex)->platformType)) { + return nextMixerProfileIndex; + } + + return -1; +} + +static void collectAutoTransitionServoTargets(bool targets[MAX_SUPPORTED_SERVOS]) +{ + memset(targets, 0, MAX_SUPPORTED_SERVOS * sizeof(targets[0])); + + const int profileIndex = getAutoTransitionServoProfileIndex(); + if (profileIndex < 0) { + return; + } + + for (int i = 0; i < MAX_SERVO_RULES; i++) { + const servoMixer_t *rule = &mixerServoMixersByIndex(profileIndex)[i]; + + if (rule->rate == 0) { + break; + } + + if (!isAutoTransitionTargetInputSource(rule->inputSource)) { + continue; + } + +#ifdef USE_PROGRAMMING_FRAMEWORK + if (!logicConditionGetValue(rule->conditionId)) { + continue; + } +#endif + + if (rule->targetChannel < MAX_SUPPORTED_SERVOS) { + targets[rule->targetChannel] = true; + } + } +} +#endif + void writeServos(void) { filterServos(); @@ -308,6 +382,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 +418,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,7 +467,7 @@ void servoMixer(float dT) input[INPUT_STABILIZED_THROTTLE] = mixerThrottleCommand - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] - input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing * 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 // center the RC input value around the RC middle value @@ -458,6 +568,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 +618,13 @@ void servoMixer(float dT) * allowed the situation when smix weight sum for an output was above 100 */ servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max); + +#ifdef USE_AUTO_TRANSITION + int16_t handoffOutput = 0; + if (mixerATGetServoHandoffOutput(i, servo[i], &handoffOutput)) { + servo[i] = constrain(handoffOutput, servoParams(i)->min, servoParams(i)->max); + } +#endif } } diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index aecd78fe319..0d94db20f26 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -85,6 +85,17 @@ typedef enum { INPUT_RC_CH33 = 58, INPUT_RC_CH34 = 59, INPUT_MIXER_SWITCH_HELPER = 60, +#ifdef USE_AUTO_TRANSITION + INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL = 61, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH = 62, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW = 63, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_PLUS = 64, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL_MINUS = 65, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_PLUS = 66, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH_MINUS = 67, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_PLUS = 68, + INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW_MINUS = 69, +#endif INPUT_SOURCE_COUNT } inputSource_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8996186a7b1..acc00dad24c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -6103,8 +6103,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) { @@ -6358,6 +6364,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // a lost model, but might help avoiding a crash. // Blink to grab user attention. TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (message == vtolTransitionMessage && osdVtolTransitionMessageShouldBlink()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (message == invertedInfoMessage) { TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 4767ae035d6..7a41bd9d480 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -124,6 +124,24 @@ #define OSD_MSG_ANGLEHOLD_PITCH "(ANGLEHOLD PITCH)" #define OSD_MSG_ANGLEHOLD_LEVEL "(ANGLEHOLD LEVEL)" #define OSD_MSG_MOVE_STICKS "MOVE STICKS TO ABORT" +#define OSD_MSG_VTOL_TRANS_START "VTOL TRANSITION START" +#define OSD_MSG_VTOL_TO_FW "VTOL TO FW" +#define OSD_MSG_VTOL_TO_MC "VTOL TO MC" +#define OSD_MSG_VTOL_MANUAL_TO_FW "VTOL MANUAL TO FW" +#define OSD_MSG_VTOL_MANUAL_TO_MC "VTOL MANUAL TO MC" +#define OSD_MSG_VTOL_MISSION_TO_FW "VTOL MISSION TO FW" +#define OSD_MSG_VTOL_MISSION_TO_MC "VTOL MISSION TO MC" +#define OSD_MSG_VTOL_RTH_TO_FW "VTOL RTH TO FW" +#define OSD_MSG_VTOL_LAND_TO_MC "VTOL LAND TO MC" +#define OSD_MSG_VTOL_SAFE_TO_MC "VTOL LOW SPD TO MC" +#define OSD_MSG_VTOL_FINISH_SWITCH "VTOL FINISHING SWITCH" +#define OSD_MSG_VTOL_RETRY_SCAN "VTOL RETRY SCAN" +#define OSD_MSG_VTOL_RETRY_ALIGN "VTOL RETRY ALIGN" +#define OSD_MSG_VTOL_TRANS_DONE "VTOL TRANSITION DONE" +#define OSD_MSG_VTOL_TRANS_ABORTED "VTOL TRANSITION ABORTED" +#define OSD_MSG_VTOL_AIRSPEED_TO "VTOL AIRSPEED TIMEOUT" +#define OSD_MSG_VTOL_MOVE_SW_FW "MOVE SWITCH TO FW" +#define OSD_MSG_VTOL_MOVE_SW_MC "MOVE SWITCH TO MC" #ifdef USE_DEV_TOOLS #define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index 38563ed5d88..9769b5e74c3 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -37,6 +37,9 @@ #include "fc/settings.h" +#include "flight/mixer_profile.h" + +#include "io/osd.h" #include "io/osd_canvas.h" #include "io/osd_common.h" #include "io/osd_grid.h" @@ -70,6 +73,127 @@ 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; +} +#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_NONE: + default: + return NULL; + } + } + + switch (status.phase) { + case MIXERAT_PHASE_TRANSITION_INITIALIZE: + return OSD_MSG_VTOL_TRANS_START; + + case MIXERAT_PHASE_TRANSITIONING: + return osdVtolTransitionRequestMessage(status.request, status.direction); + + case MIXERAT_PHASE_POST_SWITCH_FADE: + return OSD_MSG_VTOL_FINISH_SWITCH; + + case MIXERAT_PHASE_IDLE: + default: + break; + } +#endif + + return NULL; +} + +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 a032747effc..1b59539f0f1 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 086c9a6965f..5333846e464 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 d640e2b25c3..2bf82a9b034 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,17 @@ #define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec) #define FW_LAND_LOITER_ALT_TOLERANCE 150 +#ifdef USE_AUTO_TRANSITION +// One-shot MC->FW mission retry after airspeed-timeout: hold position, yaw scan, align to best pitot heading. +#define NAV_MIXERAT_RETRY_SCAN_STEP_CD DEGREES_TO_CENTIDEGREES(20) +#define NAV_MIXERAT_RETRY_HEADING_TOL_CD DEGREES_TO_CENTIDEGREES(5) +#define NAV_MIXERAT_RETRY_HEADING_SETTLE_MS 500 +#define NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS 6000 +#define NAV_MIXERAT_RETRY_MAX_TOTAL_MS 45000 +#define NAV_MIXERAT_MISSION_TRANSITION_TRACK_DISTANCE_CM 4000 +#define NAV_MIXERAT_MISSION_TRANSITION_ALTITUDE_FALLBACK_TOLERANCE_CM 100 +#endif + /*----------------------------------------------------------- * Compatibility for home position *-----------------------------------------------------------*/ @@ -119,7 +134,11 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif +#ifdef USE_AUTO_TRANSITION +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10); +#else PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7); +#endif PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -149,6 +168,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 +221,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, @@ -270,6 +295,54 @@ 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 retryAttempted; + uint8_t retryStage; + int32_t retryScanStartHeading; + int32_t retryTargetHeading; + int32_t retryBestHeading; + int32_t retryScannedCd; + float retryBestAirspeedCmS; + bool retryHadTrustedAirspeedSample; + timeMs_t retryStartTimeMs; + timeMs_t retryStepStartTimeMs; + timeMs_t retryHeadingReachedTimeMs; + fpVector3_t retryHoldPos; +} navMixerATMissionTransition_t; + +typedef enum { + NAV_MIXERAT_RETRY_STAGE_IDLE = 0, + NAV_MIXERAT_RETRY_STAGE_SCAN, + NAV_MIXERAT_RETRY_STAGE_ALIGN, +} navMixerATRetryStage_e; + +typedef enum { + NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS = 0, + NAV_MIXERAT_RETRY_SCAN_READY_TO_RETRY, + NAV_MIXERAT_RETRY_SCAN_FAILED, +} navMixerATRetryScanResult_e; + +typedef enum { + NAV_MISSION_VTOL_TRANSITION_NONE = 0, + NAV_MISSION_VTOL_TRANSITION_CONTINUE, + NAV_MISSION_VTOL_TRANSITION_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; +#else +static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; +#endif + static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode); static void updateDesiredRTHAltitude(void); static void resetAltitudeController(bool useTerrainFollowing); @@ -288,13 +361,27 @@ static void resetJumpCounter(void); static void clearJumpCounters(void); static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); +#ifdef USE_AUTO_TRANSITION +static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos); +#endif void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayPos(fpVector3_t * farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance); +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); @@ -347,6 +434,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState); +#ifdef USE_AUTO_TRANSITION +static bool beginNavigationFwToMcProtectionTransition(void); +#endif #ifdef USE_FW_AUTOLAND static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState); @@ -636,6 +726,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, } }, @@ -679,6 +770,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, } }, @@ -698,6 +790,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, } }, @@ -783,7 +876,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 } @@ -807,6 +903,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 } }, @@ -854,6 +951,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 } }, @@ -1044,8 +1142,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT, +#ifdef USE_AUTO_TRANSITION + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, +#endif [NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, //switch to its pending state [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, //switch to its pending state +#ifdef USE_AUTO_TRANSITION + [NAV_FSM_EVENT_MIXERAT_MISSION_RESUME] = NAV_STATE_WAYPOINT_IN_PROGRESS, +#endif } }, [NAV_STATE_MIXERAT_ABORT] = { @@ -1258,7 +1364,27 @@ 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, enable XY/YAW control to fly a straight acceleration segment. + if (mixerATState && + navMixerATPendingState == NAV_STATE_WAYPOINT_PRE_ACTION && + navMixerATMissionTransition.active && + navMixerATMissionTransition.request == MIXERAT_REQUEST_MISSION_TO_FW) { + stateFlags |= NAV_CTL_POS | NAV_CTL_YAW; + } + + // During one-shot retry scan/alignment, hold position and command heading in MC. + if (mixerATState && + navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE && + isTransitionRetryToFixedWingRequest(navMixerATMissionTransition.request)) { + stateFlags |= NAV_CTL_POS | NAV_CTL_YAW; + } +#endif + + return stateFlags; } flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) @@ -1282,6 +1408,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(); @@ -1656,6 +1788,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; } @@ -1674,7 +1812,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; } @@ -1762,6 +1914,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; @@ -1780,6 +1938,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LAN // If landing is not temporarily paused (FS only), position ok, OR within valid timeout - continue // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) { + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); + + if (navigationRTHAllowsLanding() && !navigationVtolMcProtectionLandingSettleReady(tmpHomePos)) { + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + return NAV_FSM_EVENT_NONE; + } + resetLandingDetector(); // force reset landing detector just in case updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land @@ -1799,6 +1965,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); @@ -1825,9 +1997,22 @@ 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 (!navigationVtolMcProtectionLandingSettleReady(landingSettlePos)) { + if (FLIGHT_MODE(NAV_WP_MODE)) { + setDesiredPosition(landingSettlePos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + } else { + setDesiredPosition(landingSettlePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + } + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + return NAV_FSM_EVENT_NONE; + } + #ifdef USE_FW_AUTOLAND if (STATE(AIRPLANE)) { int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8, approachSettingIdx = -1; @@ -1965,6 +2150,437 @@ 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 hasTrustedPitotAirspeed(float *airspeedCmS) +{ + if (!sensors(SENSOR_PITOT) || !pitotGetValidForAirspeed() || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE || + detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + return false; + } + + *airspeedCmS = pitot.airSpeed; + return true; +} +#else +static bool hasTrustedPitotAirspeed(float *airspeedCmS) +{ + UNUSED(airspeedCmS); + return false; +} +#endif + +static bool beginNavigationFwToMcProtectionTransition(void) +{ + if (navVtolFwToMcProtectionLatched) { + return false; + } + + const uint16_t thresholdCmS = systemConfig()->vtolFwToMcAutoSwitchAirspeed; + float airspeedCmS = 0.0f; + const bool trustedAirspeedAvailable = hasTrustedPitotAirspeed(&airspeedCmS); + + if (!mixerTransitionFwToMcProtectionTriggered(ARMING_FLAG(ARMED), STATE(AIRPLANE), thresholdCmS, trustedAirspeedAvailable, airspeedCmS)) { + return false; + } + + if (!checkMixerATRequired(MIXERAT_REQUEST_FW_TO_MC_PROTECTION)) { + return false; + } + + navMixerATRequestOverride = MIXERAT_REQUEST_FW_TO_MC_PROTECTION; + navVtolFwToMcProtectionLatched = true; + return true; +} + +static bool isTransitionRetryToFixedWingRequest(const mixerProfileATRequest_e request) +{ + return request == MIXERAT_REQUEST_MISSION_TO_FW || request == MIXERAT_REQUEST_RTH; +} + +static bool hasAirspeedSensorForTransitionRetry(void) +{ +#ifdef USE_PITOT + if (!sensors(SENSOR_PITOT) || !pitotGetValidForAirspeed() || pitotHasFailed()) { + return false; + } + + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_NONE || + detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + return false; + } + + return true; +#else + return false; +#endif +} + +static bool canRetryTransitionAfterAirspeedTimeout(const mixerProfileATRequest_e request) +{ + return navConfig()->general.vtol_transition_retry_on_airspeed_timeout && + isTransitionRetryToFixedWingRequest(request) && + hasAirspeedSensorForTransitionRetry(); +} + +static bool isTransitionToMultirotorRequest(const mixerProfileATRequest_e request) +{ + return request == MIXERAT_REQUEST_LAND || + request == MIXERAT_REQUEST_MISSION_TO_MC || + 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) && outputProfileHotSwitch(nextMixerProfileIndex); +} + +static void clearMissionVTOLTransitionState(void) +{ + navMixerATMissionTransition.active = false; + navMixerATMissionTransition.request = MIXERAT_REQUEST_NONE; + navMixerATMissionTransition.heading = posControl.actualState.yaw; + navMixerATMissionTransition.retryAttempted = false; + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + navMixerATMissionTransition.retryScanStartHeading = posControl.actualState.yaw; + navMixerATMissionTransition.retryTargetHeading = posControl.actualState.yaw; + navMixerATMissionTransition.retryBestHeading = posControl.actualState.yaw; + navMixerATMissionTransition.retryScannedCd = 0; + navMixerATMissionTransition.retryBestAirspeedCmS = -1.0f; + navMixerATMissionTransition.retryHadTrustedAirspeedSample = false; + navMixerATMissionTransition.retryStartTimeMs = 0; + navMixerATMissionTransition.retryStepStartTimeMs = 0; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; +} + +static void beginMissionTransitionRetryScan(const mixerProfileATRequest_e request) +{ + navMixerATMissionTransition.request = request; + navMixerATMissionTransition.retryAttempted = true; + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_SCAN; + navMixerATMissionTransition.retryScanStartHeading = wrap_36000(posControl.actualState.yaw); + navMixerATMissionTransition.retryTargetHeading = navMixerATMissionTransition.retryScanStartHeading; + navMixerATMissionTransition.retryBestHeading = navMixerATMissionTransition.retryScanStartHeading; + navMixerATMissionTransition.retryScannedCd = 0; + navMixerATMissionTransition.retryBestAirspeedCmS = -1.0f; + navMixerATMissionTransition.retryHadTrustedAirspeedSample = false; + navMixerATMissionTransition.retryStartTimeMs = millis(); + navMixerATMissionTransition.retryStepStartTimeMs = navMixerATMissionTransition.retryStartTimeMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; +} + +static navMixerATRetryScanResult_e updateMissionTransitionRetryScan(void) +{ + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_IDLE) { + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + const timeMs_t nowMs = millis(); + const bool overallTimedOut = (nowMs - navMixerATMissionTransition.retryStartTimeMs) >= NAV_MIXERAT_RETRY_MAX_TOTAL_MS; + if (overallTimedOut) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_FAILED; + } + + const int32_t headingError = ABS(wrap_18000(navMixerATMissionTransition.retryTargetHeading - posControl.actualState.yaw)); + const bool headingReached = headingError <= NAV_MIXERAT_RETRY_HEADING_TOL_CD; + const bool stepTimedOut = (nowMs - navMixerATMissionTransition.retryStepStartTimeMs) >= NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS; + + if (headingReached) { + if (!navMixerATMissionTransition.retryHeadingReachedTimeMs) { + navMixerATMissionTransition.retryHeadingReachedTimeMs = nowMs; + } + } else { + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + } + + if (!headingReached && !stepTimedOut) { + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + if (headingReached && + !stepTimedOut && + (nowMs - navMixerATMissionTransition.retryHeadingReachedTimeMs) < NAV_MIXERAT_RETRY_HEADING_SETTLE_MS) { + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_SCAN) { + float airspeedCmS = 0.0f; + if (hasTrustedPitotAirspeed(&airspeedCmS)) { + navMixerATMissionTransition.retryHadTrustedAirspeedSample = true; + if (airspeedCmS > navMixerATMissionTransition.retryBestAirspeedCmS) { + navMixerATMissionTransition.retryBestAirspeedCmS = airspeedCmS; + navMixerATMissionTransition.retryBestHeading = wrap_36000(posControl.actualState.yaw); + } + } + + navMixerATMissionTransition.retryScannedCd += NAV_MIXERAT_RETRY_SCAN_STEP_CD; + if (navMixerATMissionTransition.retryScannedCd >= DEGREES_TO_CENTIDEGREES(360)) { + if (!navMixerATMissionTransition.retryHadTrustedAirspeedSample) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_FAILED; + } + + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_ALIGN; + navMixerATMissionTransition.retryTargetHeading = wrap_36000(navMixerATMissionTransition.retryBestHeading); + navMixerATMissionTransition.retryStepStartTimeMs = nowMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + navMixerATMissionTransition.retryTargetHeading = + wrap_36000(navMixerATMissionTransition.retryScanStartHeading + navMixerATMissionTransition.retryScannedCd); + navMixerATMissionTransition.retryStepStartTimeMs = nowMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_ALIGN) { + if (!headingReached) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_FAILED; + } + + navMixerATMissionTransition.heading = wrap_36000(navMixerATMissionTransition.retryBestHeading); + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_READY_TO_RETRY; + } + + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; +} + +static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const navWaypoint_t *waypoint) +{ + const navMissionUserAction_e configuredUserAction = (navMissionUserAction_e)navConfig()->general.vtol_mission_transition_user_action; + const uint16_t configuredUserActionMask = missionUserActionMask(configuredUserAction); + + if (!configuredUserActionMask) { + return NAV_MISSION_VTOL_TRANSITION_NONE; + } + + // The configured USER action bit itself is the absolute target selector: + // 0 -> MC target, 1 -> FW target. + const bool transitionToFixedWing = ((((uint16_t)waypoint->p3) & configuredUserActionMask) != 0); + const mixerProfileATRequest_e requestedAction = transitionToFixedWing ? MIXERAT_REQUEST_MISSION_TO_FW : MIXERAT_REQUEST_MISSION_TO_MC; + + // 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; + return NAV_MISSION_VTOL_TRANSITION_START; +} + +static void updateMissionTransitionGuidance(void) +{ + if (navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE && + isTransitionRetryToFixedWingRequest(navMixerATMissionTransition.request) && + STATE(MULTIROTOR)) { + setDesiredPosition(&navMixerATMissionTransition.retryHoldPos, + navMixerATMissionTransition.retryTargetHeading, + NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return; + } + + if (navMixerATMissionTransition.active && + navMixerATMissionTransition.request == MIXERAT_REQUEST_MISSION_TO_FW && + STATE(MULTIROTOR)) { + fpVector3_t transitionTarget; + calculateFarAwayTarget(&transitionTarget, navMixerATMissionTransition.heading, NAV_MIXERAT_MISSION_TRANSITION_TRACK_DISTANCE_CM); + setDesiredPosition(&transitionTarget, navMixerATMissionTransition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return; + } + + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); +} + +navVtolTransitionOsdState_e navigationVtolTransitionOsdState(void) +{ + if (posControl.navState != NAV_STATE_MIXERAT_INITIALIZE && + posControl.navState != NAV_STATE_MIXERAT_IN_PROGRESS) { + return NAV_VTOL_TRANSITION_OSD_NONE; + } + + switch (navMixerATMissionTransition.retryStage) { + case NAV_MIXERAT_RETRY_STAGE_SCAN: + return NAV_VTOL_TRANSITION_OSD_RETRY_SCAN; + + case NAV_MIXERAT_RETRY_STAGE_ALIGN: + return NAV_VTOL_TRANSITION_OSD_RETRY_ALIGN; + + case NAV_MIXERAT_RETRY_STAGE_IDLE: + default: + return NAV_VTOL_TRANSITION_OSD_NONE; + } +} +#endif + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState) { /* A helper function to do waypoint-specific action */ @@ -1973,12 +2589,39 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_HOLD_TIME: case NAV_WP_ACTION_WAYPOINT: - case NAV_WP_ACTION_LAND: - calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); + case NAV_WP_ACTION_LAND: { + const navWaypoint_t * const activeWaypoint = &posControl.waypointList[posControl.activeWaypointIndex]; + + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + + calculateAndSetActiveWaypoint(activeWaypoint); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; posControl.wpAltitudeReached = false; + +#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) @@ -2029,6 +2672,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na // If no position sensor available - land immediately if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) { +#ifdef USE_AUTO_TRANSITION + if (beginNavigationFwToMcProtectionTransition()) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } +#endif + switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_HOLD_TIME: case NAV_WP_ACTION_WAYPOINT: @@ -2121,6 +2770,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); @@ -2284,7 +2939,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi return NAV_FSM_EVENT_NONE; } -navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState) { const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); @@ -2294,7 +2948,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; } @@ -2302,6 +2965,139 @@ 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 transitionTimeout = mixerATWasAbortedByAirspeedTimeout(); + const bool missionTransitionWasActive = navMixerATMissionTransition.active; + if (transitionAborted && + transitionTimeout && + !navMixerATMissionTransition.retryAttempted && + canRetryTransitionAfterAirspeedTimeout(required_action) && + STATE(MULTIROTOR) && + ((required_action == MIXERAT_REQUEST_MISSION_TO_FW && missionTransitionWasActive) || + required_action == MIXERAT_REQUEST_RTH)) { + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + beginMissionTransitionRetryScan(required_action); + updateMissionTransitionGuidance(); + return NAV_FSM_EVENT_NONE; + } + + if (transitionAborted && tryForceSwitchAfterFwToMcFailure(required_action)) { + transitionAborted = false; + } + + navigationFSMEvent_t nextEvent = NAV_FSM_EVENT_SWITCH_TO_IDLE; + if (transitionAborted) { + nextEvent = getTransitionFailEvent(required_action); + } else { + if (required_action == MIXERAT_REQUEST_FW_TO_MC_PROTECTION) { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_TRACKBACK: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH; + break; + case NAV_STATE_RTH_HEAD_HOME: + case NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LOITER_ABOVE_HOME: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH; + break; + case NAV_STATE_WAYPOINT_IN_PROGRESS: + case NAV_STATE_WAYPOINT_HOLD_TIME: + nextEvent = NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; + break; + default: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + break; + } + } else { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LANDING: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + break; + case NAV_STATE_WAYPOINT_PRE_ACTION: + if (missionTransitionWasActive) { + nextEvent = NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; + } + break; + default: + break; + } + } + } + + 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; +#else mixerProfileATRequest_e required_action; switch (navMixerATPendingState) { @@ -2339,12 +3135,17 @@ 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; } @@ -5062,6 +5863,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; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 398781fa596..2bc7459dc2a 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 @@ -648,6 +679,14 @@ typedef enum { MW_NAV_FLAG_ADJUSTING_ALTITUDE = 1 << 1, } navSystemStatus_Flags_e; +#ifdef USE_AUTO_TRANSITION +typedef enum { + NAV_VTOL_TRANSITION_OSD_NONE = 0, + NAV_VTOL_TRANSITION_OSD_RETRY_SCAN, + NAV_VTOL_TRANSITION_OSD_RETRY_ALIGN, +} navVtolTransitionOsdState_e; +#endif + typedef struct { navSystemStatus_Mode_e mode; navSystemStatus_State_e state; @@ -788,6 +827,9 @@ bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int3 */ bool navigationRTHAllowsLanding(void); bool isWaypointMissionRTHActive(void); +#ifdef USE_AUTO_TRANSITION +navVtolTransitionOsdState_e navigationVtolTransitionOsdState(void); +#endif bool rthClimbStageActiveAndComplete(void); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index d3099eb2465..576467fd116 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( @@ -731,6 +848,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())) { + setMulticopterCurrentPositionAsHoldTarget(NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); + } + // Get max speed for current NAV mode float maxSpeed = getActiveSpeed(); updatePositionVelocityController_MC(maxSpeed); @@ -770,7 +891,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 +900,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 +912,11 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs) if (acc.accADCf[Z] < 1.0f && baroAltRate < -200.0f) { const uint16_t idleThrottle = getThrottleIdleValue(); const uint16_t hoverThrottleRange = currentBatteryProfile->nav.mc.hover_throttle - idleThrottle; - return rcCommand[THROTTLE] < idleThrottle + ((navigationInAutomaticThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange); + const bool detected = rcCommand[THROTTLE] < idleThrottle + ((navigationInAutomaticThrottleMode() ? 0.8 : 0.5) * hoverThrottleRange); + if (detected) { + gSpikeDetectTimeMs = 0; + } + return detected; } } else if (acc.accADCf[Z] <= 1.0f) { gSpikeDetectTimeMs = 0; @@ -823,6 +953,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 +973,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 +1003,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 +1013,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 +1032,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 +1063,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 +1096,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 +1218,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 6f076659caf..c5440c7a7cf 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 @@ -183,6 +188,11 @@ 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 NAV_STATE_MIXERAT_IN_PROGRESS is active. The same + // state-specific slot is intentionally reused by other FSM states. + NAV_FSM_EVENT_MIXERAT_MISSION_RESUME = NAV_FSM_EVENT_STATE_SPECIFIC_4, +#endif NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5, NAV_FSM_EVENT_COUNT, @@ -573,6 +583,8 @@ void resetMulticopterAltitudeController(void); void resetMulticopterPositionController(void); void resetMulticopterHeadingController(void); void resetMulticopterBrakingMode(void); +bool navigationMulticopterBrakingActive(void); +bool navigationMulticopterBrakingBoostActive(void); bool adjustMulticopterAltitudeFromRCInput(void); bool adjustMulticopterHeadingFromRCInput(void); diff --git a/src/main/navigation/navigation_vtol_mc_protection.c b/src/main/navigation/navigation_vtol_mc_protection.c new file mode 100644 index 00000000000..65f00601390 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mc_protection.c @@ -0,0 +1,482 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" + +#include "fc/config.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/mixer_profile.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" +#include "navigation/navigation_vtol_mc_protection.h" + +#ifdef USE_AUTO_TRANSITION + +typedef enum { + VTOL_MC_PROTECT_FLAG_CONFIGURED = 1 << 0, + VTOL_MC_PROTECT_FLAG_VTOL_MC = 1 << 1, + VTOL_MC_PROTECT_FLAG_NAV_ACTIVE = 1 << 2, + VTOL_MC_PROTECT_FLAG_STABILIZED_ACTIVE = 1 << 3, + VTOL_MC_PROTECT_FLAG_CAPTURE_ACTIVE = 1 << 4, + VTOL_MC_PROTECT_FLAG_LANDING_SETTLE = 1 << 5, + VTOL_MC_PROTECT_FLAG_BAILOUT_ACTIVE = 1 << 6, + VTOL_MC_PROTECT_FLAG_RESERVE_SHRUNK = 1 << 7, + VTOL_MC_PROTECT_FLAG_SOFT_ALTITUDE = 1 << 8, + VTOL_MC_PROTECT_FLAG_COMMAND_SHAPED = 1 << 9, + VTOL_MC_PROTECT_FLAG_VELOCITY_FALLBACK = 1 << 10, +} vtolMcProtectionDebugFlags_e; + +typedef struct vtolMcProtectionRuntimeState_s { + vtolMcProtectionSettleState_t captureSettle; + vtolMcProtectionSettleState_t landingSettle; + vtolMcProtectionSettleState_t bailoutSettle; + bool captureActive; + bool softAltitudeActive; + bool landingSettleActive; + bool bailoutActive; + bool reserveShrunk; + bool stabilizedCommandShaped; + bool velocityFallbackActive; + timeMs_t bailoutStartMs; + int16_t bailoutStartThrottle; + uint16_t commandScalePermille; + int16_t safeThrottleMin; + int16_t safeThrottleMax; + int16_t protectedThrottle; +} vtolMcProtectionRuntimeState_t; + +static vtolMcProtectionRuntimeState_t vtolMcProtection = { + .commandScalePermille = 1000, +}; + +static bool navigationVtolMcProtectionConfigured(void) +{ + return (vtolMcProtectionMode_e)systemConfig()->vtolMcProtectionMode != VTOL_MC_PROTECTION_OFF; +} + +static bool navigationVtolMcProtectionPairedAirplaneProfileConfigured(void) +{ + for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) { + if (i != currentMixerProfileIndex && mixerConfigByIndex(i)->platformType == PLATFORM_AIRPLANE) { + return true; + } + } + + return platformTypeConfigured(PLATFORM_AIRPLANE); +} + +bool navigationVtolMcProtectionIsVtolMcMode(void) +{ + return vtolMcProtectionDetectVtolMcMode( + STATE(MULTIROTOR), + isMultirotorTypePlatform(currentMixerConfig.platformType), + navigationVtolMcProtectionPairedAirplaneProfileConfigured()); +} + +bool navigationVtolMcProtectionIsNavActive(void) +{ + return navigationVtolMcProtectionConfigured() && + ARMING_FLAG(ARMED) && + navigationVtolMcProtectionIsVtolMcMode(); +} + +bool navigationVtolMcProtectionVelocityUsable(void) +{ + return posControl.flags.estVelStatus == EST_TRUSTED; +} + +static bool navigationVtolMcProtectionVerticalVelocityUsable(void) +{ + return posControl.flags.estAltStatus >= EST_USABLE; +} + +uint16_t navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(void) +{ + const uint16_t absRoll = ABS(attitude.values.roll); + const uint16_t absPitch = ABS(attitude.values.pitch); + return absRoll > absPitch ? absRoll : absPitch; +} + +static uint16_t navigationVtolMcProtectionHorizontalSettleLimitCmS(void) +{ +#ifdef USE_MR_BRAKING_MODE + return vtolMcProtectionHorizontalSettleSpeedCmS(navConfig()->mc.braking_disengage_speed); +#else + return vtolMcProtectionHorizontalSettleSpeedCmS(0); +#endif +} + +static uint16_t navigationVtolMcProtectionVerticalSettleLimitCmS(void) +{ + return vtolMcProtectionVerticalSettleSpeedCmS(navConfig()->general.land_minalt_vspd); +} + +static uint16_t navigationVtolMcProtectionAttitudeSettleLimitDeciDeg(void) +{ + return vtolMcProtectionSettleAttitudeLimitDeciDeg(navConfig()->mc.max_bank_angle); +} + +static uint16_t navigationVtolMcProtectionBailoutLimitDeciDeg(void) +{ + return vtolMcProtectionBailoutAngleLimitDeciDeg(navConfig()->mc.max_bank_angle); +} + +static bool navigationVtolMcProtectionSettleConditionsMet(uint16_t *settleTimeMs) +{ + const bool horizontalVelocityUsable = navigationVtolMcProtectionVelocityUsable(); + const bool verticalVelocityUsable = navigationVtolMcProtectionVerticalVelocityUsable(); + + vtolMcProtection.velocityFallbackActive = vtolMcProtectionUsingVelocityFallback(horizontalVelocityUsable, verticalVelocityUsable); + *settleTimeMs = vtolMcProtectionSettleTimeMs(horizontalVelocityUsable); + + return vtolMcProtectionSettleConditionsMetWithFallback( + horizontalVelocityUsable, + verticalVelocityUsable, + posControl.actualState.velXY, + navGetCurrentActualPositionAndVelocity()->vel.z, + navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(), + navigationVtolMcProtectionHorizontalSettleLimitCmS(), + navigationVtolMcProtectionVerticalSettleLimitCmS(), + navigationVtolMcProtectionAttitudeSettleLimitDeciDeg()); +} + +static void navigationVtolMcProtectionPublishDebug(void) +{ + uint32_t flags = 0; + + if (navigationVtolMcProtectionConfigured()) { + flags |= VTOL_MC_PROTECT_FLAG_CONFIGURED; + } + if (navigationVtolMcProtectionIsVtolMcMode()) { + flags |= VTOL_MC_PROTECT_FLAG_VTOL_MC; + } + if (navigationVtolMcProtectionIsNavActive()) { + flags |= VTOL_MC_PROTECT_FLAG_NAV_ACTIVE; + } + if ((vtolMcProtectionMode_e)systemConfig()->vtolMcProtectionMode == VTOL_MC_PROTECTION_NAV_AND_STABILIZED && + ARMING_FLAG(ARMED) && navigationVtolMcProtectionIsVtolMcMode() && + (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + flags |= VTOL_MC_PROTECT_FLAG_STABILIZED_ACTIVE; + } + if (vtolMcProtection.captureActive) { + flags |= VTOL_MC_PROTECT_FLAG_CAPTURE_ACTIVE; + } + if (vtolMcProtection.landingSettleActive) { + flags |= VTOL_MC_PROTECT_FLAG_LANDING_SETTLE; + } + if (vtolMcProtection.bailoutActive) { + flags |= VTOL_MC_PROTECT_FLAG_BAILOUT_ACTIVE; + } + if (vtolMcProtection.reserveShrunk) { + flags |= VTOL_MC_PROTECT_FLAG_RESERVE_SHRUNK; + } + if (vtolMcProtection.softAltitudeActive) { + flags |= VTOL_MC_PROTECT_FLAG_SOFT_ALTITUDE; + } + if (vtolMcProtection.stabilizedCommandShaped) { + flags |= VTOL_MC_PROTECT_FLAG_COMMAND_SHAPED; + } + if (vtolMcProtection.velocityFallbackActive && + (vtolMcProtection.captureActive || vtolMcProtection.landingSettleActive || vtolMcProtection.bailoutActive)) { + flags |= VTOL_MC_PROTECT_FLAG_VELOCITY_FALLBACK; + } + + uint16_t debugProgress = 1000; + if (vtolMcProtection.captureActive) { + debugProgress = vtolMcProtection.captureSettle.elapsedMs; + } else if (vtolMcProtection.landingSettleActive) { + debugProgress = vtolMcProtection.landingSettle.elapsedMs; + } else if (vtolMcProtection.bailoutActive) { + debugProgress = vtolMcProtection.bailoutSettle.elapsedMs; + } else if (vtolMcProtection.stabilizedCommandShaped) { + debugProgress = vtolMcProtection.commandScalePermille; + } + + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 0, (int32_t)flags); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 1, vtolMcProtection.safeThrottleMin); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 2, vtolMcProtection.safeThrottleMax); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 3, vtolMcProtection.protectedThrottle); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 4, (int32_t)lrintf(posControl.actualState.velXY)); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 5, (int32_t)lrintf(navGetCurrentActualPositionAndVelocity()->vel.z)); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 6, navigationVtolMcProtectionMaxAbsAttitudeDeciDeg()); + DEBUG_SET(DEBUG_VTOL_MC_PROTECT, 7, debugProgress); +} + +void navigationVtolMcProtectionResetTransientStates(void) +{ + vtolMcProtection.captureSettle.stableSinceMs = 0; + vtolMcProtection.captureSettle.elapsedMs = 0; + vtolMcProtection.landingSettle.stableSinceMs = 0; + vtolMcProtection.landingSettle.elapsedMs = 0; + vtolMcProtection.bailoutSettle.stableSinceMs = 0; + vtolMcProtection.bailoutSettle.elapsedMs = 0; + vtolMcProtection.captureActive = false; + vtolMcProtection.softAltitudeActive = false; + vtolMcProtection.landingSettleActive = false; + vtolMcProtection.bailoutActive = false; + vtolMcProtection.reserveShrunk = false; + vtolMcProtection.stabilizedCommandShaped = false; + vtolMcProtection.velocityFallbackActive = false; + vtolMcProtection.commandScalePermille = 1000; + vtolMcProtection.protectedThrottle = 0; +} + +vtolMcProtectionThrottleBounds_t navigationVtolMcProtectionGetThrottleBounds(const int16_t idleThrottle, const int16_t hoverThrottle, const int16_t maxThrottle) +{ + const bool active = navigationVtolMcProtectionIsNavActive() && navigationInAutomaticThrottleMode(); + vtolMcProtectionThrottleBounds_t bounds = vtolMcProtectionComputeThrottleBounds( + active, + idleThrottle, + hoverThrottle, + maxThrottle, + systemConfig()->vtolMcThrReservePercent); + + vtolMcProtection.safeThrottleMin = bounds.min; + vtolMcProtection.safeThrottleMax = bounds.max; + vtolMcProtection.reserveShrunk = bounds.reserveShrunk; + + if (!active) { + vtolMcProtection.bailoutActive = false; + vtolMcProtection.softAltitudeActive = false; + vtolMcProtection.velocityFallbackActive = false; + } + + navigationVtolMcProtectionPublishDebug(); + return bounds; +} + +static bool navigationVtolMcProtectionBailoutEntryCondition(void) +{ + return navigationVtolMcProtectionIsNavActive() && + navigationInAutomaticThrottleMode() && + navigationVtolMcProtectionMaxAbsAttitudeDeciDeg() >= navigationVtolMcProtectionBailoutLimitDeciDeg(); +} + +bool navigationVtolMcProtectionShouldFreezeAltitudeIntegrator(void) +{ + return vtolMcProtection.bailoutActive || + vtolMcProtection.softAltitudeActive || + navigationVtolMcProtectionBailoutEntryCondition(); +} + +static int16_t navigationVtolMcProtectionBailoutTargetThrottle(const int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, const int16_t hoverThrottle) +{ + int16_t targetThrottle = constrain(hoverThrottle, bounds->min, bounds->max); + const float verticalSpeedCmS = navGetCurrentActualPositionAndVelocity()->vel.z; + + if (verticalSpeedCmS < -(float)navigationVtolMcProtectionVerticalSettleLimitCmS() && targetThrottle < requestedThrottle) { + targetThrottle = requestedThrottle; + } + + return constrain(targetThrottle, bounds->min, bounds->max); +} + +int16_t navigationVtolMcProtectionApplyBailoutThrottle(const int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, const int16_t hoverThrottle) +{ + const bool entryCondition = navigationVtolMcProtectionBailoutEntryCondition(); + const timeMs_t nowMs = millis(); + + if (!entryCondition && vtolMcProtection.bailoutActive) { + uint16_t settleTimeMs; + const bool settleConditionsMet = navigationVtolMcProtectionSettleConditionsMet(&settleTimeMs); + const bool readyToExit = vtolMcProtectionUpdateSettleState( + &vtolMcProtection.bailoutSettle, + settleConditionsMet, + settleTimeMs, + nowMs); + + if (readyToExit) { + vtolMcProtection.bailoutActive = false; + vtolMcProtection.bailoutSettle.stableSinceMs = 0; + vtolMcProtection.bailoutSettle.elapsedMs = 0; + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + vtolMcProtection.protectedThrottle = requestedThrottle; + navigationVtolMcProtectionPublishDebug(); + return requestedThrottle; + } + } + + if (!entryCondition && !vtolMcProtection.bailoutActive) { + vtolMcProtection.protectedThrottle = requestedThrottle; + navigationVtolMcProtectionPublishDebug(); + return requestedThrottle; + } + + if (!vtolMcProtection.bailoutActive) { + vtolMcProtection.bailoutActive = true; + vtolMcProtection.bailoutStartMs = nowMs; + vtolMcProtection.bailoutStartThrottle = requestedThrottle; + vtolMcProtection.bailoutSettle.stableSinceMs = 0; + vtolMcProtection.bailoutSettle.elapsedMs = 0; + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + } + + const int16_t targetThrottle = navigationVtolMcProtectionBailoutTargetThrottle(requestedThrottle, bounds, hoverThrottle); + const uint16_t rampTimeMs = VTOL_MC_SETTLE_TIME_MS > VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS ? + VTOL_MC_SETTLE_TIME_MS : + VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS; + const float progress = constrainf((nowMs - vtolMcProtection.bailoutStartMs) / (float)rampTimeMs, 0.0f, 1.0f); + const int16_t protectedThrottle = lrintf(vtolMcProtection.bailoutStartThrottle + + (targetThrottle - vtolMcProtection.bailoutStartThrottle) * progress); + + vtolMcProtection.protectedThrottle = constrain(protectedThrottle, bounds->min, bounds->max); + navigationVtolMcProtectionPublishDebug(); + return vtolMcProtection.protectedThrottle; +} + +bool navigationVtolMcProtectionApplyCapture(const uint32_t navStateFlags) +{ + const bool captureAllowed = navigationVtolMcProtectionIsNavActive() && + (navStateFlags & NAV_CTL_POS) && + (navStateFlags & NAV_CTL_HOLD); + + if (!captureAllowed) { + vtolMcProtection.captureActive = false; + vtolMcProtection.captureSettle.stableSinceMs = 0; + vtolMcProtection.captureSettle.elapsedMs = 0; + navigationVtolMcProtectionPublishDebug(); + return false; + } + + uint16_t settleTimeMs; + const bool settleConditionsMet = navigationVtolMcProtectionSettleConditionsMet(&settleTimeMs); + const bool ready = vtolMcProtectionUpdateSettleState( + &vtolMcProtection.captureSettle, + settleConditionsMet, + settleTimeMs, + millis()); + + vtolMcProtection.captureActive = !ready; + navigationVtolMcProtectionPublishDebug(); + return vtolMcProtection.captureActive; +} + +bool navigationVtolMcProtectionApplySoftAltitudeCapture(const uint32_t navStateFlags) +{ + const bool holdOrTransitionState = ((navStateFlags & NAV_CTL_POS) && (navStateFlags & NAV_CTL_HOLD)) || + (navStateFlags & NAV_MIXERAT); + const bool shouldRelaxAltitude = vtolMcProtectionShouldRelaxAltitude( + navigationVtolMcProtectionIsNavActive(), + navigationInAutomaticThrottleMode(), + navigationVtolMcProtectionVelocityUsable(), + holdOrTransitionState, + navStateFlags & NAV_CTL_LAND, + navStateFlags & NAV_CTL_EMERG, + posControl.flags.isAdjustingAltitude, + posControl.actualState.velXY, + navigationVtolMcProtectionHorizontalSettleLimitCmS(), + vtolMcProtection.captureActive); + + vtolMcProtection.softAltitudeActive = shouldRelaxAltitude; + + if (shouldRelaxAltitude) { + // While a winged VTOL is bleeding horizontal speed, avoid chasing a stale + // altitude lock point; stabilize vertical rate and re-lock after settling. + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); + } + + navigationVtolMcProtectionPublishDebug(); + return shouldRelaxAltitude; +} + +bool navigationVtolMcProtectionLandingSettleReady(const fpVector3_t *landingPos) +{ + if (!navigationVtolMcProtectionIsNavActive()) { + vtolMcProtection.landingSettleActive = false; + vtolMcProtection.landingSettle.stableSinceMs = 0; + vtolMcProtection.landingSettle.elapsedMs = 0; + navigationVtolMcProtectionPublishDebug(); + return true; + } + + const uint16_t landingCaptureRadiusCm = vtolMcProtectionLandingCaptureRadiusCm(navConfig()->general.waypoint_radius); + const bool insideLandingRadius = calculateDistanceToDestination(landingPos) <= landingCaptureRadiusCm; + uint16_t settleTimeMs; + const bool settleConditionsMet = navigationVtolMcProtectionSettleConditionsMet(&settleTimeMs); + const bool ready = vtolMcProtectionUpdateSettleState( + &vtolMcProtection.landingSettle, + insideLandingRadius && settleConditionsMet, + settleTimeMs, + millis()); + + vtolMcProtection.landingSettleActive = !ready; + navigationVtolMcProtectionPublishDebug(); + return ready; +} + +void navigationVtolMcProtectionApplyStabilizedCommandShaping(int16_t *rollCommand, int16_t *pitchCommand, int16_t *yawCommand) +{ + vtolMcProtection.stabilizedCommandShaped = false; + vtolMcProtection.commandScalePermille = 1000; + + if ((vtolMcProtectionMode_e)systemConfig()->vtolMcProtectionMode != VTOL_MC_PROTECTION_NAV_AND_STABILIZED || + !ARMING_FLAG(ARMED) || + !navigationVtolMcProtectionIsVtolMcMode() || + !(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) || + !navigationVtolMcProtectionVelocityUsable()) { + navigationVtolMcProtectionPublishDebug(); + return; + } + + const float commandScale = vtolMcProtectionCommandScaleForSpeed(posControl.actualState.velXY); + vtolMcProtection.commandScalePermille = vtolMcProtectionCommandScalePermille(commandScale); + + if (commandScale >= 0.999f) { + navigationVtolMcProtectionPublishDebug(); + return; + } + + const int16_t originalRoll = *rollCommand; + const int16_t originalPitch = *pitchCommand; + const int16_t originalYaw = *yawCommand; + + *rollCommand = vtolMcProtectionApplyCommandScale(*rollCommand, commandScale); + *pitchCommand = vtolMcProtectionApplyCommandScale(*pitchCommand, commandScale); + *yawCommand = vtolMcProtectionApplyCommandScale(*yawCommand, commandScale); + + vtolMcProtection.stabilizedCommandShaped = *rollCommand != originalRoll || + *pitchCommand != originalPitch || + *yawCommand != originalYaw; + navigationVtolMcProtectionPublishDebug(); +} + +void navigationVtolMcProtectionPublishThrottleDebug(const vtolMcProtectionThrottleBounds_t *bounds, const int16_t protectedThrottle) +{ + vtolMcProtection.safeThrottleMin = bounds->min; + vtolMcProtection.safeThrottleMax = bounds->max; + vtolMcProtection.reserveShrunk = bounds->reserveShrunk; + vtolMcProtection.protectedThrottle = protectedThrottle; + navigationVtolMcProtectionPublishDebug(); +} + +#endif diff --git a/src/main/navigation/navigation_vtol_mc_protection.h b/src/main/navigation/navigation_vtol_mc_protection.h new file mode 100644 index 00000000000..3d7c59b6498 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mc_protection.h @@ -0,0 +1,89 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#include "platform.h" + +#include "common/time.h" +#include "common/vector.h" + +#include "navigation/navigation_vtol_mc_protection_logic.h" + +#ifdef USE_AUTO_TRANSITION +bool navigationVtolMcProtectionIsVtolMcMode(void); +bool navigationVtolMcProtectionIsNavActive(void); +bool navigationVtolMcProtectionVelocityUsable(void); +uint16_t navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(void); + +vtolMcProtectionThrottleBounds_t navigationVtolMcProtectionGetThrottleBounds(int16_t idleThrottle, int16_t hoverThrottle, int16_t maxThrottle); +bool navigationVtolMcProtectionShouldFreezeAltitudeIntegrator(void); +int16_t navigationVtolMcProtectionApplyBailoutThrottle(int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, int16_t hoverThrottle); + +bool navigationVtolMcProtectionApplyCapture(uint32_t navStateFlags); +bool navigationVtolMcProtectionApplySoftAltitudeCapture(uint32_t navStateFlags); +bool navigationVtolMcProtectionLandingSettleReady(const fpVector3_t *landingPos); +void navigationVtolMcProtectionApplyStabilizedCommandShaping(int16_t *rollCommand, int16_t *pitchCommand, int16_t *yawCommand); +void navigationVtolMcProtectionPublishThrottleDebug(const vtolMcProtectionThrottleBounds_t *bounds, int16_t protectedThrottle); +void navigationVtolMcProtectionResetTransientStates(void); +#else +static inline bool navigationVtolMcProtectionIsVtolMcMode(void) { return false; } +static inline bool navigationVtolMcProtectionIsNavActive(void) { return false; } +static inline bool navigationVtolMcProtectionVelocityUsable(void) { return false; } +static inline uint16_t navigationVtolMcProtectionMaxAbsAttitudeDeciDeg(void) { return 0; } +static inline vtolMcProtectionThrottleBounds_t navigationVtolMcProtectionGetThrottleBounds(int16_t idleThrottle, int16_t hoverThrottle, int16_t maxThrottle) +{ + return vtolMcProtectionComputeThrottleBounds(false, idleThrottle, hoverThrottle, maxThrottle, 0); +} +static inline bool navigationVtolMcProtectionShouldFreezeAltitudeIntegrator(void) { return false; } +static inline int16_t navigationVtolMcProtectionApplyBailoutThrottle(int16_t requestedThrottle, const vtolMcProtectionThrottleBounds_t *bounds, int16_t hoverThrottle) +{ + (void)bounds; + (void)hoverThrottle; + return requestedThrottle; +} +static inline bool navigationVtolMcProtectionApplyCapture(uint32_t navStateFlags) +{ + (void)navStateFlags; + return false; +} +static inline bool navigationVtolMcProtectionApplySoftAltitudeCapture(uint32_t navStateFlags) +{ + (void)navStateFlags; + return false; +} +static inline bool navigationVtolMcProtectionLandingSettleReady(const fpVector3_t *landingPos) +{ + (void)landingPos; + return true; +} +static inline void navigationVtolMcProtectionApplyStabilizedCommandShaping(int16_t *rollCommand, int16_t *pitchCommand, int16_t *yawCommand) +{ + (void)rollCommand; + (void)pitchCommand; + (void)yawCommand; +} +static inline void navigationVtolMcProtectionPublishThrottleDebug(const vtolMcProtectionThrottleBounds_t *bounds, int16_t protectedThrottle) +{ + (void)bounds; + (void)protectedThrottle; +} +static inline void navigationVtolMcProtectionResetTransientStates(void) {} +#endif diff --git a/src/main/navigation/navigation_vtol_mc_protection_logic.h b/src/main/navigation/navigation_vtol_mc_protection_logic.h new file mode 100644 index 00000000000..fed788d50eb --- /dev/null +++ b/src/main/navigation/navigation_vtol_mc_protection_logic.h @@ -0,0 +1,365 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#include "common/time.h" + +#define VTOL_MC_LANDING_CAPTURE_RADIUS_CAP_CM 100 +#define VTOL_MC_VERTICAL_SETTLE_SPEED_CAP_CM_S 100 +#define VTOL_MC_SETTLE_ATTITUDE_CAP_DEG 20 +#define VTOL_MC_BAILOUT_ANGLE_EXTRA_DEG 15 +#define VTOL_MC_BAILOUT_ANGLE_MIN_DEG 45 +#define VTOL_MC_BAILOUT_ANGLE_MAX_DEG 60 +#define VTOL_MC_COMMAND_SHAPE_START_CM_S 300 +#define VTOL_MC_COMMAND_SHAPE_FULL_CM_S 800 +#define VTOL_MC_COMMAND_SHAPE_MIN_SCALE 0.5f +#define VTOL_MC_SETTLE_TIME_MS 1000 +#define VTOL_MC_FALLBACK_SETTLE_TIME_MS 3000 +#define VTOL_MC_BAILOUT_MIN_RAMP_TIME_MS 500 +#define VTOL_MC_DEFAULT_BRAKING_DISENGAGE_CM_S 75 +#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 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..f90b02afb35 --- /dev/null +++ b/src/main/navigation/navigation_vtol_mission_logic.h @@ -0,0 +1,72 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +typedef enum { + NAV_MISSION_VTOL_PRECONDITION_READY = 0, + NAV_MISSION_VTOL_PRECONDITION_WAIT, + NAV_MISSION_VTOL_PRECONDITION_REJECT, +} navMissionVtolTransitionPrecondition_e; + +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; +} diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 3f32fcee861..c893ee247b6 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -682,6 +682,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) { @@ -891,6 +898,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 183c0cfa054..274f3a5a097 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -156,6 +156,11 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED, // cm/s // 47 LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION, // deg // 48 LOGIC_CONDITION_OPERAND_FLIGHT_RELATIVE_WIND_OFFSET, // deg // 49 +#ifdef USE_AUTO_TRANSITION + LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_ROLL, // 50 + LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_PITCH, // 51 + LOGIC_CONDITION_OPERAND_FLIGHT_AUTOTRANSITION_TARGET_STABILIZED_YAW, // 52 +#endif } logicFlightOperands_e; typedef enum { diff --git a/src/main/target/common.h b/src/main/target/common.h index bd7730226f6..056ee8375d0 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -204,8 +204,9 @@ #define USE_TELEMETRY_SBUS2 #endif -//Designed to free space of F722 and F411 MCUs +// Keep larger optional features off 512 KB targets to preserve flash space. #if (MCU_FLASH_SIZE > 512) +#define USE_AUTO_TRANSITION #define USE_TELEMETRY_SIM #define USE_VTX_FFPV #define USE_SERIALRX_SUMD @@ -231,4 +232,3 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER - diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 5b43b636a77..7d8012de616 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -69,6 +69,11 @@ set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TE set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c") set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER) +set_property(SOURCE mixer_transition_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE mixer_transition_policy_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE mixer_transition_scenarios_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE navigation_vtol_mission_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) +set_property(SOURCE vtol_mc_protection_logic_unittest.cc PROPERTY definitions USE_AUTO_TRANSITION) function(unit_test src) get_filename_component(basename ${src} NAME) diff --git a/src/test/unit/mixer_transition_logic_unittest.cc b/src/test/unit/mixer_transition_logic_unittest.cc new file mode 100644 index 00000000000..947c9eab4dd --- /dev/null +++ b/src/test/unit/mixer_transition_logic_unittest.cc @@ -0,0 +1,491 @@ +#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, 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, 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, + false, + true, + 1, + 1)); + + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + false, + false, + false, + 1, + 1)); +} + +TEST(MixerTransitionLogicTest, NavigationHandbackClearsForNewNavigationOrExplicitTransition) +{ + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + true, + false, + false, + 0, + 1)); + + EXPECT_TRUE(mixerTransitionNavigationHandbackShouldClear( + false, + true, + true, + 0, + 1)); + + EXPECT_FALSE(mixerTransitionNavigationHandbackShouldHoldProfile( + true, + false, + true, + true, + false, + 0, + 1)); +} + +TEST(MixerTransitionLogicTest, LegacySessionIgnoresAutoControllerAfterProfileHotSwitch) +{ + mixerTransitionManualSessionMode_e sessionMode = MIXER_TRANSITION_MANUAL_SESSION_NONE; + + 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, 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..e5a51285909 --- /dev/null +++ b/src/test/unit/mixer_transition_policy_unittest.cc @@ -0,0 +1,606 @@ +#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, HandoffProgressUsesAirspeedDirectlyWhenAvailable) +{ + EXPECT_FLOAT_EQ(0.35f, mixerTransitionResolveHandoffProgress(true, true, 0.80f, 0.35f)); +} + +TEST(MixerTransitionPolicyTest, HandoffProgressPreservesPreviousPeakWhenAirspeedDropsOut) +{ + EXPECT_FLOAT_EQ(0.62f, mixerTransitionResolveHandoffProgress(true, false, 0.62f, 0.41f)); + EXPECT_FLOAT_EQ(0.91f, mixerTransitionResolveHandoffProgress(true, false, 0.62f, 0.91f)); +} + +TEST(MixerTransitionPolicyTest, MotorRampProgressFallsBackToFullWhenDisabledOrZeroRamp) +{ + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeMotorRampProgress(false, 500, 100)); + EXPECT_FLOAT_EQ(1.0f, mixerTransitionComputeMotorRampProgress(true, 0, 100)); + EXPECT_FLOAT_EQ(0.40f, mixerTransitionComputeMotorRampProgress(true, 500, 200)); +} + +TEST(MixerTransitionPolicyTest, 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, HotSwitchWithoutAirspeedCompletesImmediatelyWhenTimerIsZero) +{ + const mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_FW, + 1500, + false, + 0.0f, + false, + 0.0f, + 0, + 0); + + EXPECT_TRUE(progress.readyForHotSwitch); + EXPECT_FALSE(progress.usedAirspeed); + EXPECT_FALSE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(1.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, HotSwitchCapturesAndReusesStartAirspeedForFwToMc) +{ + mixerTransitionHotSwitchProgress_t progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1600.0f, + false, + 0.0f, + 0, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_TRUE(progress.usedAirspeed); + EXPECT_TRUE(progress.transitionStartAirspeedCaptured); + EXPECT_FLOAT_EQ(1600.0f, progress.transitionStartAirspeedCmS); + EXPECT_FLOAT_EQ(0.0f, progress.progress); + + progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 1300.0f, + progress.transitionStartAirspeedCaptured, + progress.transitionStartAirspeedCmS, + 0, + 1000); + + EXPECT_FALSE(progress.readyForHotSwitch); + EXPECT_FLOAT_EQ(0.50f, progress.progress); + + progress = mixerTransitionEvaluateHotSwitch( + MIXERAT_DIRECTION_TO_MC, + 1000, + true, + 900.0f, + progress.transitionStartAirspeedCaptured, + progress.transitionStartAirspeedCmS, + 0, + 1000); + + EXPECT_TRUE(progress.readyForHotSwitch); + EXPECT_FLOAT_EQ(1.0f, progress.progress); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskCapturesOldLiftAndNewPusherButNotSharedTiltMotors) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; // shared tilt motor + currentMixer[1].throttle = 1.0f; // old lift motor + targetMixer[0].throttle = 1.0f; // shared tilt motor + targetMixer[2].throttle = 1.0f; // FW pusher appears after switch + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 3, + currentMixer, + targetMixer); + + EXPECT_EQ((1U << 1) | (1U << 2), fadeMask.motorMask); + EXPECT_EQ((1U << 2), fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskCapturesOnlyOldPusherWhenReturningToMultirotor) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; // shared tilt motor + currentMixer[2].throttle = 1.0f; // FW pusher disappears after switch + targetMixer[0].throttle = 1.0f; // shared tilt motor + targetMixer[1].throttle = 1.0f; // MC lift motor + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_MC, + false, + 3, + currentMixer, + targetMixer); + + EXPECT_EQ((1U << 2), fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskStaysEmptyWhenDynamicScalingIsDisabled) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; + targetMixer[1].throttle = 1.0f; + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + false, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 2, + currentMixer, + targetMixer); + + EXPECT_EQ(0U, fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, PostSwitchFadeMaskStaysEmptyForSharedTiltOnlyConfiguration) +{ + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS] = {}; + motorMixer_t targetMixer[MAX_SUPPORTED_MOTORS] = {}; + + currentMixer[0].throttle = 1.0f; + targetMixer[0].throttle = 1.0f; + + const mixerTransitionPostSwitchFadeMask_t fadeMask = mixerTransitionComputePostSwitchFadeMask( + true, + 500, + MIXERAT_DIRECTION_TO_FW, + true, + 1, + currentMixer, + targetMixer); + + EXPECT_EQ(0U, fadeMask.motorMask); + EXPECT_EQ(0U, fadeMask.toCurrentMotorMask); +} + +TEST(MixerTransitionPolicyTest, AutoServoInputMayDecreaseDuringFwToMc) +{ + EXPECT_EQ(100, mixerTransitionUpdateServoInput( + 500, + false, + true, + true, + false, + false, + 0.20f)); +} diff --git a/src/test/unit/mixer_transition_scenarios_unittest.cc b/src/test/unit/mixer_transition_scenarios_unittest.cc new file mode 100644 index 00000000000..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..e1cb1c79f9a --- /dev/null +++ b/src/test/unit/navigation_vtol_mission_logic_unittest.cc @@ -0,0 +1,137 @@ +#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)); +} 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..5eb8c424713 --- /dev/null +++ b/src/test/unit/vtol_mc_protection_logic_unittest.cc @@ -0,0 +1,397 @@ +#include + +extern "C" { +#include "navigation/navigation_vtol_mc_protection_logic.h" +} + +TEST(VtolMcProtectionLogicTest, DetectsOnlyPairedMultirotorVtolMcMode) +{ + EXPECT_TRUE(vtolMcProtectionDetectVtolMcMode(true, true, true)); + EXPECT_FALSE(vtolMcProtectionDetectVtolMcMode(false, true, true)); + EXPECT_FALSE(vtolMcProtectionDetectVtolMcMode(true, false, true)); + EXPECT_FALSE(vtolMcProtectionDetectVtolMcMode(true, true, false)); +} + +TEST(VtolMcProtectionLogicTest, DefaultOffKeepsLegacyThrottleBounds) +{ + const vtolMcProtectionThrottleBounds_t bounds = vtolMcProtectionComputeThrottleBounds( + false, + 1000, + 1500, + 2000, + 15); + + EXPECT_EQ(1000, bounds.min); + EXPECT_EQ(2000, bounds.max); + EXPECT_FALSE(bounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, AppliesThrottleReserveBeforePidBounds) +{ + const vtolMcProtectionThrottleBounds_t bounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1500, + 2000, + 15); + + EXPECT_EQ(1150, bounds.min); + EXPECT_EQ(1850, bounds.max); + EXPECT_FALSE(bounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, ShrinksReserveToKeepHoverThrottleReachable) +{ + const vtolMcProtectionThrottleBounds_t lowHoverBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1100, + 2000, + 20); + + EXPECT_EQ(1100, lowHoverBounds.min); + EXPECT_EQ(1800, lowHoverBounds.max); + EXPECT_TRUE(lowHoverBounds.reserveShrunk); + + const vtolMcProtectionThrottleBounds_t highHoverBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1900, + 2000, + 20); + + EXPECT_EQ(1200, highHoverBounds.min); + EXPECT_EQ(1900, highHoverBounds.max); + EXPECT_TRUE(highHoverBounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, ThrottleBoundsHandleDisabledReserveAndInvalidRange) +{ + const vtolMcProtectionThrottleBounds_t zeroReserveBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1000, + 1500, + 2000, + 0); + + EXPECT_EQ(1000, zeroReserveBounds.min); + EXPECT_EQ(2000, zeroReserveBounds.max); + EXPECT_FALSE(zeroReserveBounds.reserveShrunk); + + const vtolMcProtectionThrottleBounds_t invalidRangeBounds = vtolMcProtectionComputeThrottleBounds( + true, + 1500, + 1500, + 1400, + 20); + + EXPECT_EQ(1500, invalidRangeBounds.min); + EXPECT_EQ(1400, invalidRangeBounds.max); + EXPECT_FALSE(invalidRangeBounds.reserveShrunk); +} + +TEST(VtolMcProtectionLogicTest, SettleTimerRequiresContinuousStableConditions) +{ + vtolMcProtectionSettleState_t state = {}; + + EXPECT_EQ(1000, VTOL_MC_SETTLE_TIME_MS); + EXPECT_EQ(3000, VTOL_MC_FALLBACK_SETTLE_TIME_MS); + EXPECT_EQ(VTOL_MC_SETTLE_TIME_MS, vtolMcProtectionSettleTimeMs(true)); + EXPECT_EQ(VTOL_MC_FALLBACK_SETTLE_TIME_MS, vtolMcProtectionSettleTimeMs(false)); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 100)); + EXPECT_EQ(0, state.elapsedMs); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 600)); + EXPECT_EQ(500, state.elapsedMs); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, false, VTOL_MC_SETTLE_TIME_MS, 700)); + EXPECT_EQ(0, state.elapsedMs); + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 800)); + EXPECT_TRUE(vtolMcProtectionUpdateSettleState(&state, true, VTOL_MC_SETTLE_TIME_MS, 1800)); +} + +TEST(VtolMcProtectionLogicTest, SettleTimerSaturatesElapsedTime) +{ + vtolMcProtectionSettleState_t state = {}; + + EXPECT_FALSE(vtolMcProtectionUpdateSettleState(&state, true, UINT16_MAX, 1)); + EXPECT_TRUE(vtolMcProtectionUpdateSettleState(&state, true, UINT16_MAX, 70000)); + EXPECT_EQ(UINT16_MAX, state.elapsedMs); +} + +TEST(VtolMcProtectionLogicTest, SettleConditionsUseDerivedThresholds) +{ + EXPECT_EQ(75, vtolMcProtectionHorizontalSettleSpeedCmS(0)); + EXPECT_EQ(50, vtolMcProtectionHorizontalSettleSpeedCmS(50)); + EXPECT_EQ(50, vtolMcProtectionVerticalSettleSpeedCmS(50)); + EXPECT_EQ(100, vtolMcProtectionVerticalSettleSpeedCmS(500)); + EXPECT_EQ(200, vtolMcProtectionSettleAttitudeLimitDeciDeg(35)); + EXPECT_EQ(150, vtolMcProtectionSettleAttitudeLimitDeciDeg(15)); + + EXPECT_TRUE(vtolMcProtectionSettleConditionsMet(70.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMet(80.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMet(70.0f, -120.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMet(70.0f, -50.0f, 250, 75, 100, 200)); +} + +TEST(VtolMcProtectionLogicTest, VelocityFallbackSettleUsesVerticalSpeedAndAttitudeOnly) +{ + EXPECT_FALSE(vtolMcProtectionUsingVelocityFallback(true, true)); + EXPECT_FALSE(vtolMcProtectionUsingVelocityFallback(false, false)); + EXPECT_TRUE(vtolMcProtectionUsingVelocityFallback(false, true)); + + EXPECT_TRUE(vtolMcProtectionSettleConditionsMetWithFallback( + false, true, 500.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + false, false, 0.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + false, true, 0.0f, -120.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + false, true, 0.0f, -50.0f, 250, 75, 100, 200)); + + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + true, true, 500.0f, -50.0f, 150, 75, 100, 200)); + EXPECT_FALSE(vtolMcProtectionSettleConditionsMetWithFallback( + true, false, 0.0f, 0.0f, 0, 75, 100, 200)); +} + +TEST(VtolMcProtectionLogicTest, LandingCaptureRadiusCapsLargeWaypointRadius) +{ + EXPECT_EQ(100, vtolMcProtectionLandingCaptureRadiusCm(10000)); + EXPECT_EQ(80, vtolMcProtectionLandingCaptureRadiusCm(80)); +} + +TEST(VtolMcProtectionLogicTest, SoftAltitudeRelaxationOnlyAppliesDuringCaptureOrTransition) +{ + EXPECT_TRUE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, false, 120.0f, 75, false)); + EXPECT_TRUE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, false, 20.0f, 75, true)); + + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + false, true, true, true, false, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, false, true, true, false, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, false, true, false, false, false, 120.0f, 75, false)); + EXPECT_TRUE(vtolMcProtectionShouldRelaxAltitude( + true, true, false, true, false, false, false, 120.0f, 75, true)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, false, false, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, true, false, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, true, false, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, true, 120.0f, 75, false)); + EXPECT_FALSE(vtolMcProtectionShouldRelaxAltitude( + true, true, true, true, false, false, false, 75.0f, 75, false)); +} + +TEST(VtolMcProtectionLogicTest, 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, 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)); +}