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..6a6eb44cc2b 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -6,21 +6,68 @@ 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 airspeed gets too low. After that switch, iNAV stays in MC until you deliberately command another manual profile change. + +This behavior is controlled by `mixer_vtol_manualswitch_autotransition_controller`. +Turn it ON in both mixer profiles if you want the same switch behavior in both directions. +If it is OFF, manual transition keeps the older behavior. + +In Active Modes: + +- `MIXER TRANSITION` shows that the internal transition logic is actually running. +- `MIXER PROFILE 2` shows that mixer profile 2 is currently active. + +There are two separate manual paths: + +- `MIXER PROFILE 2` is still a direct manual profile switch when `MIXER TRANSITION` is OFF. +- `MIXER TRANSITION` starts the smooth automatic transition sequence when `mixer_vtol_manualswitch_autotransition_controller = ON`. +- If both are ON together while the automatic transition controller is enabled, the controller temporarily owns the profile switching. When `MIXER TRANSITION` turns OFF again, direct `MIXER PROFILE 2` switching becomes active again. +- If low-speed protection switches the model from FW to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. -The use of Transition Mode is recommended to enable further features and future developments like fail-safe support. Mapping motor to servo output, or servo with logic conditions is **not** recommended +3-position switch example: + +- This example assumes the usual VTOL order used in this document: + - Profile 1 = FW + - Profile 2 = MC +- One supported mapping is: + - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) + - Pos2 = Transition request (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) + - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) +- Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. +- If you intentionally swap the profile order, keep the same idea and swap the FW and MC end positions. ## Servo @@ -56,7 +103,11 @@ It is recommend that the pilot uses a RC mode switch to activate modes or switch Profile files Switching is not available until the runtime sensor calibration is done. Switching is NOT available when navigation mode is activate. `mixer_profile` 1 will be used as default, `mixer_profile` 2 will be used when the `MIXER PROFILE 2` mode box is activated. -Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. Here is sample of using these RC modes: +Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. + +Another supported mapping is where one switch position turns ON both `MIXER PROFILE 2` and `MIXER TRANSITION`. +With `mixer_vtol_manualswitch_autotransition_controller = OFF`, `MIXER TRANSITION` is used as a live transition input. +With `mixer_vtol_manualswitch_autotransition_controller = ON`, that same overlap position is used as a controller-owned transition position. ![Alt text](Screenshots/mixer_profile.png) @@ -66,11 +117,240 @@ 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. + +### 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 hot-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 fade down as fixed-wing authority is reduced and motor stabilisation comes back in. +These inputs are active only while the smooth autotransition 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 transition-progress servo movement such as tilt or helper servos. + +How `mixer_vtol_transition_scale_ramp_time_ms` works: + +- Motor ramp-in: + - MC->FW: forward motor power ramps from `0 -> 100%` over this time. + - FW->MC: lift motor power ramps from `vtol_transition_lift_min_percent -> 100%` over this time. + - `= 0` (default): those motor-power changes happen immediately. +- This timer does not decide when the transition completes. +- Lift motor reduction in MC->FW, plus MC/FW control handoff in both directions: + - 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`). + +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, lift motor power returns to full in about `1.2s`, +- when pitot is working, control handover still follows airspeed, +- if pitot stops being usable, handover 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 handover to reduce abrupt transitions. + +#### Test 3 - Mission-authorized transition (mission integration) + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` +- `set mixer_switch_trans_timer = 50` +- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` +- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `set vtol_transition_lift_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`) +- `debug[1]` = active request (`MIXERAT_REQUEST_*` enum value) +- `debug[2]` = packed transition flags: + - bits 0-1: transition direction (`0=NONE`, `1=TO_FW`, `2=TO_MC`) + - bit2: auto-transition controller active + - bit3: transition mixing output active (`isMixerTransitionMixing`) + - bit4: RC `MIXERTRANSITION` mode active + - bit5: airspeed-controlled path in use + - bit6: profile change done + - bit7: transition aborted + - bit8: manual VTOL auto-transition controller enabled in current mixer config + - bit9: dynamic transition mixer enabled in current mixer config + - bits 10-11: current mixer profile index + - bits 12-13: next mixer profile index + - bit14: manual transition currently allowed by navigation state + - bit15: mission mode active + - bit16: transition mixing requested (`isMixerTransitionMixing_requested`) + - bit17: failsafe mode active + - bit18: manual VTOL auto-transition controller effective after mission gating + - bit19: RC `MIXERPROFILE` mode active +- `debug[3]` = progress x1000 (`0..1000`) +- `debug[4]` = pusher scale x1000 (`0..1000`) +- `debug[5]` = lift scale x1000 (`0..1000`) +- `debug[6]` = MC stabilisation scale x1000 (`0..1000`) +- `debug[7]` = current mixer profile pitch transition PID multiplier (`transition_PID_mmix_multiplier_pitch`) ## TailSitter (planned for INAV 7.1) TailSitter is supported by add a 90deg offset to the board alignment. Set the board aliment normally in the mixer_profile for FW mode(`set platform_type = AIRPLANE`), The motor trust axis should be same direction as the airplane nose. Then, in the mixer_profile for takeoff and landing set `tailsitter_orientation_offset = ON ` to apply orientation offset. orientation offset will also add a 45deg orientation offset. diff --git a/docs/Navigation.md b/docs/Navigation.md index 4d7b9740d91..f73d894c981 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -102,6 +102,46 @@ 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. + +Safety and scope: + +- This path uses authorized automated transition state handling; it does not permit manual mixer profile switching during normal waypoint navigation. +- It still depends on valid mixer profile switching infrastructure (two configured mixer profiles and a valid `MIXER PROFILE 2` mode activation condition). + `wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`). `wp reset` - Resets the list, sets the number of waypoints to 0 and marks the list as invalid (but doesn't delete the waypoint definitions). diff --git a/docs/Settings.md b/docs/Settings.md index cbc526adb33..e597a2fe6f7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -794,6 +794,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) | LULU | | | SBUS2 | | | OSD_REFRESH | | +| VTOL_TRANSITION | | --- @@ -3211,7 +3212,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, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing whenever trusted pitot is not usable. | Default | Min | Max | | --- | --- | --- | @@ -3219,6 +3220,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 changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mixer_vtol_transition_scale_ramp_time_ms + +When smooth VTOL transition power changes are ON, this controls motor ramp-in time only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the lift motors from their configured minimum back to full power. `0` applies those motor-power changes immediately. This timer does not decide when the transition completes and it does not control the multicopter/fixed-wing control handoff. Handoff still follows trusted pitot airspeed when pitot is usable, otherwise `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. @@ -4636,6 +4677,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 +7098,66 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, --- +### vtol_fw_to_mc_auto_switch_airspeed_cm_s + +Extra low-speed protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. Available only on targets with more than 512 KB flash. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20000 | + +--- + +### 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. In FW->MC, it fades down from full strength to this value. 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 fades down to this value. In FW->MC, lift power starts from this value and rises back to full power. `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, multicopter stabilisation fades down to this value. In FW->MC, it starts from this value and rises back to full stabilisation. `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..2ded36f0208 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -23,15 +23,21 @@ We highly value your feedback as it plays a crucial role in the development and # VTOL Configuration Steps ### The VTOL functionality is achieved by switching/transitioning between two configurations stored in the FC. VTOL specific configurations are Mixer Profiles with associated control profiles. One profile set is for fixed-wing(FW) mode, One is for multi-copter(MC) mode. Configuration/Settings other than Mixer/control profiles are shared among two modes + +This guide uses the long-standing VTOL setup order: +- Profile 1 = fixed-wing (FW) +- Profile 2 = multicopter (MC) + +The firmware can work with the profiles swapped, but keeping one order in the guide makes the setup steps easier to follow. ![Alt text](Screenshots/mixerprofile_flow.png) 0. **Find a DIFF ALL file for your model and start from there if possible** - Be aware that `MIXER PROFILE 2` RC mode setting introduced by diff file can get your stuck in a mixer_profile. remove or change channel to proceed 1. **Setup Profile 1:** - - Configure it as a normal fixed-wing/multi-copter. + - Configure it as your normal fixed-wing setup. 2. **Setup Profile 2:** - - Configure it as a normal multi-copter/fixed-wing. + - Configure it as your normal multicopter setup. 3. **Mode Tab Settings:** - Set up switching in the mode tab. @@ -125,8 +131,8 @@ save save ``` -2. **Configure the fixed-wing/Multi-Copter:** - - Configure your fixed-wing/Multi-Copter as you normally would, or you can copy and paste default settings to expedite the process. +2. **Configure the fixed-wing:** + - Configure your fixed-wing as you normally would, or you can copy and paste default settings to speed things up. - Dshot esc protocol availability might be limited depends on outputs and fc board you are using. change the motor wiring or use oneshot/multishot esc protocol and calibrate throttle range. - You can use throttle = -1 as a placeholder for the motor you wish to stop if the motor isn't the last motor - Consider conducting a test flight to ensure that everything operates as expected. And tune the settings, trim the servos. @@ -147,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. @@ -283,6 +295,324 @@ set mixer_automated_switch = ON If you set `mixer_automated_switch` to `OFF` for all mixer profiles (the default setting), the model will not perform automated transitions. You can always enable navigation modes after performing a manual transition. +## Unified VTOL Transition Controller (Manual + Mission) + +This feature is available only on targets with more than 512 KB flash. +In standard INAV builds those targets are compiled with `USE_AUTO_TRANSITION`. +Targets with 512 KB flash keep the older VTOL mixer transition behavior and do not include the smooth auto-transition settings. + +INAV now uses one internal VTOL transition controller for both: +- manual `MIXER TRANSITION` requests, and +- mission-authorized VTOL transitions. + +This keeps one safety boundary for profile changes and avoids separate transition implementations. + +### Behavior summary + +- iNAV always tracks transition progress internally. +- If valid pitot airspeed is available, airspeed is the main way iNAV decides when transition is complete. +- If pitot is not available, iNAV falls back to a timer. +- Ground speed is not used for transition completion. +- Mission VTOL transition uses the same controller and does not directly drive the motors by itself. +- During normal waypoint navigation, manual `MIXER PROFILE` and `MIXER TRANSITION` switching is still blocked. +- `MIXER PROFILE 2` is still a direct manual profile switch when you are flying manually. +- Smooth automatic transition is started by `MIXER TRANSITION` when the manual auto-controller is ON, or by a mission transition request. + +### Manual transition semantics + +This does not remove the older manual behavior. The older behavior is still available if you want it. + +With `mixer_vtol_manualswitch_autotransition_controller = ON`: +- Turn this ON in both mixer profiles if you want the same behavior in both directions. +- Each time `MIXER TRANSITION` moves from OFF to ON, iNAV starts one transition. +- After it starts, the transition keeps running until the speed target or timer target is reached. +- Leaving the switch ON does not keep restarting the transition. +- To start another transition, turn the switch OFF and then ON again. +- If you turn the switch OFF before the profile change happens, that transition request is cancelled. +- Optional extra protection: set `vtol_fw_to_mc_auto_switch_airspeed_cm_s > 0` if you want FW->MC to start automatically when pitot airspeed becomes too low. After that switch, iNAV stays in MC until you deliberately command another manual profile change. + +With `mixer_vtol_manualswitch_autotransition_controller = OFF`: +- the older manual behavior is preserved. + +Typical 3-position switch workflow: +- Position 1: FW +- Position 2: Transition request +- Position 3: MC + +Operational example: +- fly in MC (pos3) -> move to Transition (pos2) to start automatic MC->FW transition -> after completion move to FW (pos1) +- reverse the order for FW->MC + +Important RC mapping constraint: +- One supported mapping is: + - Pos1 = FW (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` OFF) + - Pos2 = Transition trigger (`MIXER PROFILE 2` OFF, `MIXER TRANSITION` ON) + - Pos3 = MC (`MIXER PROFILE 2` ON, `MIXER TRANSITION` OFF) +- Keep `mixer_vtol_manualswitch_autotransition_controller` ON in both profiles used by this mapping. +- Another supported mapping is the overlap version: while both `MIXER PROFILE 2` and `MIXER TRANSITION` are ON, the transition controller owns the switching until `MIXER TRANSITION` turns OFF again. + +### Mission-authorized transition semantics + +Mission transition is configured with `nav_vtol_mission_transition_user_action`. + +- `OFF`: feature disabled. +- `USER1`..`USER4`: the selected USER flag becomes the flight-mode selector on navigable waypoints. +- selected flag `0` -> target MC profile +- selected flag `1` -> target FW profile +- when this feature is ON, every navigable waypoint should intentionally have that USER flag either clear or set +- Mission progression pauses during transition and resumes only after completion. +- If the aircraft is already in the requested mode, iNAV does nothing and continues. + +For MC -> FW mission transition: +- guidance uses a straight acceleration run, +- normal waypoint advancement is paused during transition. + +### Airspeed-first completion logic + +MC -> FW: +- `vtol_transition_to_fw_min_airspeed_cm_s` is the target airspeed. +- If pitot stops being usable, or if this is `0`, MC->FW uses `mixer_switch_trans_timer` instead. + +FW -> MC: +- `vtol_transition_to_mc_max_airspeed_cm_s` is the airspeed that must be reached or lower. +- If pitot stops being usable, or if this is `0`, FW->MC uses `mixer_switch_trans_timer` instead. + +Timeout: +- `mixer_switch_trans_timer` is the original VTOL transition timer. It is still the backup completion timer when trusted pitot airspeed is not being used. +- `mixer_vtol_transition_airspeed_timeout_ms` is only a maximum wait time for the required airspeed while pitot is still usable. It does not complete the transition by itself; it aborts that airspeed-controlled attempt. +- If pitot stops being usable, iNAV stops using the airspeed timeout and falls back to `mixer_switch_trans_timer`. +- For pitot-based setups, use a non-zero `mixer_switch_trans_timer` as a sensible backup time, typically `40..60` (`4..6s`). + +### Smooth power changes during transition + +When `mixer_vtol_transition_dynamic_mixer = ON`, iNAV can smoothly change: +- forward motor power, +- lift motor power, +- multicopter stabilisation strength, +- fixed-wing control strength. + +When `mixer_vtol_transition_dynamic_mixer = OFF`, the older static behavior is preserved. +When it is 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 hot-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 fade down as fixed-wing authority is reduced and motor stabilisation comes back in. +These inputs are active only while the smooth autotransition 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 transition-progress servo movement such as tilt or helper servos. + +`mixer_vtol_transition_scale_ramp_time_ms` controls motor ramp-in timing when this feature is ON. +It does not decide when the transition completes. + +How `mixer_vtol_transition_scale_ramp_time_ms` works: +- Motor ramp-in: + - MC->FW: forward motor power ramps from `0 -> 100%` over this time. + - FW->MC: lift motor power ramps from `vtol_transition_lift_min_percent -> 100%` over this time. + - `= 0` (default): those motor-power changes happen immediately. +- Lift motor reduction in MC->FW, plus MC/FW control handoff in both directions: + - with valid pitot airspeed, they still follow airspeed-based transition progress. + - if pitot is not usable, they fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). + +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, lift motor power returns to full in about `1.2s`, +- when pitot is working, control handover still follows airspeed, +- if pitot is not usable, handover 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. + +### Example test presets (VTOL ~1.0m wingspan, ~1720g AUW) + +These are example starting points for initial testing. They are not universal values; tune after bench tests and short flight tests. + +#### Test 1 - Legacy-compatible baseline (manual transition check) + +Goal: +- Verify that the new controller does not change legacy behavior when smooth power changes are disabled. +- Good first test after flashing. + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = OFF` +- `set mixer_switch_trans_timer = 45` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 0` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 900` +- `set mixer_vtol_transition_airspeed_timeout_ms = 0` +- `set mixer_vtol_transition_scale_ramp_time_ms = 0` +- `set nav_vtol_mission_transition_user_action = OFF` + +What this does: +- Keeps transition mixing behavior close to legacy mode. +- Uses timer-driven completion when no trusted pitot threshold is configured. +- Uses conservative FW->MC completion threshold. +- Disables mission-authorized transition while validating manual behavior. + +#### Test 2 - Airspeed-first + smooth power changes (manual transition tuning) + +Goal: +- Enable the full new behavior: airspeed-first completion and smooth forward-motor and control handover. + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` +- `set mixer_switch_trans_timer = 50` +- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` +- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `set vtol_transition_lift_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` + +What this does: +- MC->FW completes primarily on pitot airspeed (1300 cm/s), with timer fallback only if pitot is unavailable/unhealthy. +- FW->MC completes when airspeed drops to 850 cm/s. +- In MC->FW, the forward motor ramps to full power in `1.2s` while lift power and control handover still follow airspeed progress. +- The pusher ramp is quick enough (1.2 s) to reduce step torque while still allowing strong acceleration. +- Timeout abort protects against staying too long in airspeed-controlled transition without reaching threshold. + +#### Test 3 - Mission-authorized transition (end-to-end mission flow) + +Goal: +- Validate mission User Action integration and pause/resume behavior. + +CLI: +- `set mixer_vtol_manualswitch_autotransition_controller = ON` +- `set mixer_vtol_transition_dynamic_mixer = ON` +- `set vtol_transition_to_fw_min_airspeed_cm_s = 1300` +- `set vtol_transition_to_mc_max_airspeed_cm_s = 850` +- `set mixer_switch_trans_timer = 50` +- `set mixer_vtol_transition_airspeed_timeout_ms = 6500` +- `set mixer_vtol_transition_scale_ramp_time_ms = 1200` +- `set vtol_transition_lift_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` + +What this does: +- Uses USER1 as the absolute per-waypoint target selector: + - USER1 bit clear -> target MC + - USER1 bit set -> target FW +- Pauses mission progression during transition and resumes after completion. +- Uses a straight MC->FW acceleration segment (no loiter) before the switch to fixed-wing. +- Adds a minimum altitude gate (12 m) before mission transition starts. + +### Detailed effect of the three percentage settings + +These three settings are active only when `mixer_vtol_transition_dynamic_mixer = ON`. + +1. `vtol_transition_lift_min_percent` +- Sets the lowest lift motor power used during transition. +- MC -> FW: lift power goes from `100%` at start down to `lift_min_percent`. +- FW -> MC: lift power goes from `lift_min_percent` at start up to `100%`. + +Example (`vtol_transition_lift_min_percent = 20`): +- MC -> FW at 50% progress: lift power is about `60%`. +- FW -> MC at 50% progress: lift power is about `60%`. + +2. `vtol_transition_mc_authority_min_percent` +- Sets the lowest multicopter stabilisation used during transition. +- MC -> FW: MC stabilisation goes from `100%` at start down to `mc_authority_min_percent`. +- FW -> MC: MC stabilisation goes from `mc_authority_min_percent` at start up to `100%`. + +Example (`vtol_transition_mc_authority_min_percent = 30`): +- MC -> FW at 50% progress: MC stabilisation is about `65%`. +- FW -> MC at 50% progress: MC stabilisation is about `65%`. + +3. `vtol_transition_fw_authority_min_percent` +- Sets the lowest fixed-wing control used during transition. +- MC -> FW: fixed-wing control goes from `fw_authority_min_percent` at start up to `100%`. +- FW -> MC: fixed-wing control goes from `100%` at start down to `fw_authority_min_percent`. +- During MC -> FW, this same setting also scales `INPUT_AUTOTRANSITION_TARGET_STABILIZED_*` servo rules configured in the MC mixer profile. +- During FW -> MC, the same setting scales down the matching FW servo stabilisation on the outputs marked by those MC mixer rules. + +Example (`vtol_transition_fw_authority_min_percent = 25`): +- MC -> FW at 50% progress: fixed-wing control is about `62.5%`. +- FW -> MC at 50% progress: fixed-wing control is about `62.5%`. + +Practical note: +- `vtol_transition_fw_authority_min_percent = 100` keeps full fixed-wing control through the whole transition. +- Lower values bring fixed-wing control in and out more gently. + +## Setting Scope (Important) + +The new VTOL settings are split into two groups: + +### Per-mixer-profile settings + +In the examples in this guide, mixer profile 1 is FW and mixer profile 2 is MC. +These settings can differ between the two mixer profiles: + +- `mixer_automated_switch` +- `mixer_switch_trans_timer` +- `mixer_vtol_transition_dynamic_mixer` +- `mixer_vtol_manualswitch_autotransition_controller` +- `mixer_vtol_transition_airspeed_timeout_ms` +- `mixer_vtol_transition_scale_ramp_time_ms` + +### Global settings + +These are shared system-wide and are not profile-specific: + +- `vtol_transition_to_fw_min_airspeed_cm_s` +- `vtol_transition_to_mc_max_airspeed_cm_s` +- `vtol_fw_to_mc_auto_switch_airspeed_cm_s` +- `vtol_transition_lift_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` + +## CLI Commands (English) + +Use these commands in CLI (`set ...`, then `save`): + +- `set mixer_vtol_manualswitch_autotransition_controller = ON|OFF` + - Makes `MIXER TRANSITION` start one automatic transition each time you turn it ON. + +- `set mixer_vtol_transition_dynamic_mixer = ON|OFF` + - Turns smooth transition power changes ON or OFF. + +- `set vtol_transition_to_fw_min_airspeed_cm_s = ` + - Preferred MC -> FW completion threshold (pitot airspeed). + +- `set mixer_switch_trans_timer = ` + - Backup transition time used when pitot airspeed is not available. + +- `set vtol_transition_to_mc_max_airspeed_cm_s = ` + - FW -> MC completion threshold (pitot airspeed). + +- `set vtol_fw_to_mc_auto_switch_airspeed_cm_s = ` + - Optional low-speed protection threshold for fixed-wing. After it switches to MC, iNAV stays in MC until you deliberately command another manual profile change (`0` disables). + +- `set mixer_vtol_transition_airspeed_timeout_ms = ` + - How long iNAV waits for required pitot airspeed before aborting. + +- `set mixer_vtol_transition_scale_ramp_time_ms = ` + - Ramp-in time for the MC->FW forward motor and the FW->MC lift motors. + +- `set vtol_transition_lift_min_percent = <0..100>` + - Lowest lift motor power used during transition. + +- `set vtol_transition_mc_authority_min_percent = <0..100>` + - Lowest multicopter stabilisation used during transition. + +- `set vtol_transition_fw_authority_min_percent = <0..100>` + - Lowest fixed-wing control used during transition. + +- `set nav_vtol_mission_transition_user_action = OFF|USER1|USER2|USER3|USER4` + - Selects which waypoint USER flag tells iNAV to use MC or FW at each waypoint. + +- `set nav_vtol_mission_transition_min_altitude_cm = ` + - Optional minimum altitude before mission transition may start (`0` disables). + +Mission profile-switch dependency: +- Mission VTOL transition uses the existing profile-change path, so two valid mixer profiles and a configured `MIXER PROFILE 2` mode activation condition are required. + # Notes and Experiences ## General @@ -297,3 +627,47 @@ 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` + +Motor ramp-in and control handover are 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, lift motor power uses the same timer to rise from `vtol_transition_lift_min_percent` back to full power; if this is `0`, that lift power returns immediately. +This timer does not decide when the transition completes. +Lift motor reduction in MC->FW, plus MC stabilisation and FW control handoff in both directions, still prefer pitot-based transition progress whenever pitot is working. +If pitot is not usable, those handoff changes fall back to the normal transition timer/progress behavior (`mixer_switch_trans_timer`). + +For transition/pusher motors (`-2.0 < throttle < -1.0`), output is interpolated from idle to target: + +`motor = idle + (target - idle) * pusherScale` + +where: +- `target = -mixerThrottle * 1000` +- `idle = throttleRangeMin` + +If pitot is unavailable/unhealthy, timer fallback is used (`mixer_switch_trans_timer`). diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f5225ca3e1e..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..8e585d299ab 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -80,6 +80,7 @@ typedef enum { DEBUG_LULU, DEBUG_SBUS2, DEBUG_OSD_REFRESH, + DEBUG_VTOL_TRANSITION, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 27713297c86..42bd968a24a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -227,7 +227,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "GPS", "LULU", "SBUS2", - "OSD_REFRESH" + "OSD_REFRESH", + "VTOL_TRANSITION" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d3021317ae5..8cdca161e34 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, 9); +#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,14 @@ 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, +#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..b22e292b032 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -78,6 +78,14 @@ 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; +#endif char craftName[MAX_NAME_LENGTH + 1]; char pilotName[MAX_NAME_LENGTH + 1]; } systemConfig_t; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 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..e41b8247aa3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,7 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", - "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH"] + "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "VTOL_TRANSITION"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e @@ -171,6 +171,15 @@ tables: - name: nav_wp_mission_restart enum: navMissionRestart_e values: ["START", "RESUME", "SWITCH"] + - name: nav_wp_user_action + enum: navMissionUserAction_e + values: ["OFF", "USER1", "USER2", "USER3", "USER4"] + - name: nav_vtol_transition_fail_action_mc_to_fw + enum: navVtolTransitionFailActionMcToFw_e + values: ["IDLE", "POSH", "RTH", "EMERGENCY_LANDING"] + - name: nav_vtol_transition_fail_action_fw_to_mc + enum: navVtolTransitionFailActionFwToMc_e + values: ["IDLE", "LOITER", "RTH", "EMERGENCY_LANDING", "FORCE_SWITCH"] - name: djiRssiSource values: ["RSSI", "CRSF_LQ"] enum: djiRssiSource_e @@ -1269,11 +1278,37 @@ groups: field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." + description: "Original VTOL transition timer, still used as the backup completion time. If trusted pitot airspeed is not being used, iNAV completes the transition from this timer instead. With smooth VTOL transition power changes ON, lift motor power, multicopter stabilisation, and fixed-wing control handoff also fall back to this timing 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 changes. This affects forward motor ramp-up, lift motor power reduction, multicopter stabilisation reduction, and fixed-wing control fade-in. Used by both manual `MIXER TRANSITION` and mission-requested VTOL transitions. 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: "When smooth VTOL transition power changes are ON, this controls motor ramp-in time only. In MC->FW it ramps the forward motor from idle to full target power. In FW->MC it ramps the lift motors from their configured minimum back to full power. `0` applies those motor-power changes immediately. This timer does not decide when the transition completes and it does not control the multicopter/fixed-wing control handoff. Handoff still follows trusted pitot airspeed when pitot is usable, otherwise `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 @@ -2621,6 +2656,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 +4031,48 @@ 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 protection for fixed-wing flight [cm/s]. If airspeed falls to this value or lower while in FW, iNAV automatically starts FW->MC. After the switch to MC, iNAV keeps the MC profile until you deliberately command another manual profile change. Used only when `mixer_vtol_manualswitch_autotransition_controller` is ON. Set to 0 to disable. 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 fades down to this value. In FW->MC, lift power starts from this value and rises back to full power. `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, multicopter stabilisation fades down to this value. In FW->MC, it starts from this value and rises back to full stabilisation. `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. In FW->MC, it fades down from full strength to this value. 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: name description: "Craft name" default_value: "" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80992b772d..10a96272c42 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -48,6 +48,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" @@ -520,10 +521,17 @@ 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 mcAuthorityScale = mixerATGetMcAuthorityScale(); + input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f) * mcAuthorityScale; + input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f) * mcAuthorityScale; + input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f) * mcAuthorityScale; +#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 } } @@ -625,7 +633,15 @@ 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. 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 float liftScale = isMixerTransitionMixing ? mixerATGetLiftScale() : 1.0f; + if (currentMixer[i].throttle > 0.0f) { + motorThrottle *= liftScale; + } +#endif + + motor[i] = rpyMix[i] + constrain(motorThrottle, throttleMin, throttleMax); if (failsafeIsActive()) { motor[i] = constrain(motor[i], motorConfig()->mincommand, getMaxThrottle()); @@ -639,7 +655,14 @@ void FAST_CODE mixTable(void) } //spin stopped motors only in mixer transition mode if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && !feature(FEATURE_REVERSIBLE_MOTORS)) { +#ifdef USE_AUTO_TRANSITION + const float pusherScale = mixerATGetPusherScale(); + const float pusherTarget = -currentMixer[i].throttle * 1000.0f; + const float pusherIdle = throttleRangeMin; + motor[i] = pusherIdle + (pusherTarget - pusherIdle) * pusherScale; +#else motor[i] = -currentMixer[i].throttle * 1000; +#endif motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } } @@ -737,4 +760,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..745f0a3c360 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -10,6 +10,7 @@ #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" #include "drivers/time.h" +#include "common/maths.h" #include "flight/mixer.h" #include "common/axis.h" #include "flight/pid.h" @@ -17,6 +18,8 @@ #include "flight/failsafe.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "sensors/pitotmeter.h" +#include "sensors/sensors.h" #include "fc/fc_core.h" #include "fc/config.h" @@ -29,6 +32,7 @@ #include "navigation/navigation.h" #include "common/log.h" +#include "build/debug.h" mixerConfig_t currentMixerConfig; int currentMixerProfileIndex; @@ -36,8 +40,19 @@ bool isMixerTransitionMixing; bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; int nextMixerProfileIndex; +#ifdef USE_AUTO_TRANSITION +static bool manualTransitionModeWasActive; +static bool manualTransitionReadyForEdge = true; +static bool manualTransitionSessionLatched; +static bool manualFwToMcProtectionLatched; +#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 +68,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, @@ -108,10 +129,279 @@ void mixerConfigInit(void) 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.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; +#else mixerProfileAT.transitionStartTime = millis(); mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; +#endif +} + +#ifdef USE_AUTO_TRANSITION +static bool requestTransitionsToFixedWing(const mixerProfileATRequest_e required_action) +{ + return required_action == MIXERAT_REQUEST_RTH || + required_action == MIXERAT_REQUEST_MISSION_TO_FW || + required_action == MIXERAT_REQUEST_MANUAL_TO_FW; +} + +static mixerProfileATDirection_e directionForRequest(const mixerProfileATRequest_e required_action) +{ + if (requestTransitionsToFixedWing(required_action)) { + return MIXERAT_DIRECTION_TO_FW; + } + + if (required_action == MIXERAT_REQUEST_LAND || + required_action == MIXERAT_REQUEST_MISSION_TO_MC || + required_action == MIXERAT_REQUEST_MANUAL_TO_MC) { + return MIXERAT_DIRECTION_TO_MC; + } + + return MIXERAT_DIRECTION_NONE; +} + +static void resetTransitionScales(void) +{ + mixerProfileAT.progress = 0.0f; + mixerProfileAT.handoffScalingProgress = 0.0f; + mixerProfileAT.blendToFw = 0.0f; + mixerProfileAT.pusherScale = 0.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; +} + +static void setLegacyTransitionScales(void) +{ + mixerProfileAT.progress = 1.0f; + mixerProfileAT.handoffScalingProgress = 1.0f; + mixerProfileAT.blendToFw = 1.0f; + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; +} + +static float blendScale(float from, float to, float progress) +{ + return from + (to - from) * constrainf(progress, 0.0f, 1.0f); } +static float getMotorRampProgress(void) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer) { + return 1.0f; + } + + if (currentMixerConfig.vtolTransitionScaleRampTimeMs <= 0) { + return 1.0f; + } + + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + return constrainf((float)elapsedMs / (float)currentMixerConfig.vtolTransitionScaleRampTimeMs, 0.0f, 1.0f); +} + +static float getHandoffScalingProgress(void) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer) { + return 1.0f; + } + + if (mixerProfileAT.usedAirspeed) { + mixerProfileAT.handoffScalingProgress = constrainf(mixerProfileAT.progress, 0.0f, 1.0f); + return mixerProfileAT.handoffScalingProgress; + } + + // Preserve already-applied handoff scaling if pitot drops out mid-transition. + // Without trusted pitot, handoff returns to the normal transition timer/progress behavior. + mixerProfileAT.handoffScalingProgress = MAX(mixerProfileAT.handoffScalingProgress, constrainf(mixerProfileAT.progress, 0.0f, 1.0f)); + return mixerProfileAT.handoffScalingProgress; +} + +static bool hasTrustedPitotAirspeed(float *airspeedCmS) +{ +#ifdef USE_PITOT + if (!sensors(SENSOR_PITOT) || !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 uint16_t getAirspeedThresholdForDirection(const mixerProfileATDirection_e direction) +{ + if (direction == MIXERAT_DIRECTION_TO_FW) { + return systemConfig()->vtolTransitionToFwMinAirspeed; + } + + if (direction == MIXERAT_DIRECTION_TO_MC) { + return systemConfig()->vtolTransitionToMcMaxAirspeed; + } + + return 0; +} + +static bool shouldRequestManualFwToMcProtection(const bool manualControllerEnabled) +{ + if (!manualControllerEnabled || !STATE(AIRPLANE)) { + return false; + } + + const uint16_t thresholdCmS = systemConfig()->vtolFwToMcAutoSwitchAirspeed; + if (thresholdCmS == 0 || !hasPitotSensorForManualProtection()) { + return false; + } + + float airspeedCmS = 0.0f; + if (!hasTrustedPitotAirspeed(&airspeedCmS)) { + return false; + } + + return airspeedCmS <= thresholdCmS; +} + +static void updateTransitionScales(void) +{ + if (!currentMixerConfig.vtolTransitionDynamicMixer) { + mixerProfileAT.blendToFw = 1.0f; + mixerProfileAT.pusherScale = 1.0f; + mixerProfileAT.liftScale = 1.0f; + mixerProfileAT.mcAuthorityScale = 1.0f; + mixerProfileAT.fwAuthorityScale = 1.0f; + return; + } + + const float liftFloor = constrainf(systemConfig()->vtolTransitionLiftMinPercent / 100.0f, 0.0f, 1.0f); + const float mcFloor = constrainf(systemConfig()->vtolTransitionMcAuthorityMinPercent / 100.0f, 0.0f, 1.0f); + const float fwFloor = constrainf(systemConfig()->vtolTransitionFwAuthorityMinPercent / 100.0f, 0.0f, 1.0f); + const float handoffProgress = getHandoffScalingProgress(); + + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + const float pusherProgress = getMotorRampProgress(); + + mixerProfileAT.pusherScale = blendScale(0.0f, 1.0f, pusherProgress); + mixerProfileAT.liftScale = blendScale(1.0f, liftFloor, handoffProgress); + mixerProfileAT.mcAuthorityScale = blendScale(1.0f, mcFloor, handoffProgress); + mixerProfileAT.fwAuthorityScale = blendScale(fwFloor, 1.0f, handoffProgress); + } else if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + const float liftRampProgress = getMotorRampProgress(); + + mixerProfileAT.pusherScale = blendScale(1.0f, 0.0f, handoffProgress); + mixerProfileAT.liftScale = blendScale(liftFloor, 1.0f, liftRampProgress); + mixerProfileAT.mcAuthorityScale = blendScale(mcFloor, 1.0f, handoffProgress); + mixerProfileAT.fwAuthorityScale = blendScale(1.0f, fwFloor, handoffProgress); + } + + mixerProfileAT.blendToFw = constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); +} + +static void abortTransition(const bool byAirspeedTimeout) +{ + const bool wasActive = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; + isMixerTransitionMixing_requested = false; + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.aborted = wasActive; + mixerProfileAT.abortedByAirspeedTimeout = wasActive && byAirspeedTimeout; + mixerProfileAT.hotSwitchDone = false; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + mixerProfileAT.usedAirspeed = false; + mixerProfileAT.transitionStartAirspeedCaptured = false; + mixerProfileAT.transitionStartAirspeedCmS = 0.0f; + resetTransitionScales(); +} + +static bool mixerATReadyForHotSwitch(const mixerProfileATRequest_e required_action) +{ + const mixerProfileATDirection_e direction = directionForRequest(required_action); + const uint16_t airspeedThresholdCmS = getAirspeedThresholdForDirection(direction); + const uint32_t elapsedMs = millis() - mixerProfileAT.transitionStartTime; + const uint32_t transitionTimerMs = MAX(0, currentMixerConfig.switchTransitionTimer) * 100; + float airspeedCmS = 0.0f; + + if (direction == MIXERAT_DIRECTION_NONE) { + mixerProfileAT.progress = 0.0f; + mixerProfileAT.usedAirspeed = false; + return false; + } + + if (airspeedThresholdCmS > 0 && hasTrustedPitotAirspeed(&airspeedCmS)) { + mixerProfileAT.usedAirspeed = true; + if (direction == MIXERAT_DIRECTION_TO_FW) { + mixerProfileAT.progress = constrainf(airspeedCmS / airspeedThresholdCmS, 0.0f, 1.0f); + return airspeedCmS >= airspeedThresholdCmS; + } + + if (!mixerProfileAT.transitionStartAirspeedCaptured) { + mixerProfileAT.transitionStartAirspeedCmS = airspeedCmS; + mixerProfileAT.transitionStartAirspeedCaptured = true; + } + + const float startAirspeed = mixerProfileAT.transitionStartAirspeedCmS; + const float thresholdAirspeed = airspeedThresholdCmS; + if (startAirspeed <= thresholdAirspeed) { + mixerProfileAT.progress = 1.0f; + } else { + mixerProfileAT.progress = constrainf((startAirspeed - airspeedCmS) / (startAirspeed - thresholdAirspeed), 0.0f, 1.0f); + } + + return airspeedCmS <= airspeedThresholdCmS; + } + + mixerProfileAT.usedAirspeed = false; + if (transitionTimerMs > 0) { + mixerProfileAT.progress = constrainf((float)elapsedMs / (float)transitionTimerMs, 0.0f, 1.0f); + } else { + mixerProfileAT.progress = 1.0f; + } + + return elapsedMs >= transitionTimerMs; +} +#endif + bool platformTypeConfigured(flyingPlatformType_e platformType) { if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){ @@ -120,6 +410,18 @@ bool platformTypeConfigured(flyingPlatformType_e platformType) return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType; } +#ifdef USE_AUTO_TRANSITION +static bool missionTransitionToMultirotorTypeConfigured(void) +{ + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) { + return false; + } + + const flyingPlatformType_e nextPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; + return isMultirotorTypePlatform(nextPlatformType); +} +#endif + bool checkMixerATRequired(mixerProfileATRequest_e required_action) { //return false if mixerAT condition is not required or setting is not valid @@ -132,6 +434,30 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) return false; } +#ifdef USE_AUTO_TRANSITION + switch (required_action) { + case MIXERAT_REQUEST_RTH: + return currentMixerConfig.automated_switch && STATE(MULTIROTOR) && platformTypeConfigured(PLATFORM_AIRPLANE); + + case MIXERAT_REQUEST_LAND: + return currentMixerConfig.automated_switch && STATE(AIRPLANE) && missionTransitionToMultirotorTypeConfigured(); + + case MIXERAT_REQUEST_MISSION_TO_FW: + return STATE(MULTIROTOR) && platformTypeConfigured(PLATFORM_AIRPLANE); + + case MIXERAT_REQUEST_MISSION_TO_MC: + return STATE(AIRPLANE) && missionTransitionToMultirotorTypeConfigured(); + + case MIXERAT_REQUEST_MANUAL_TO_FW: + return STATE(MULTIROTOR) && platformTypeConfigured(PLATFORM_AIRPLANE); + + case MIXERAT_REQUEST_MANUAL_TO_MC: + return STATE(AIRPLANE) && missionTransitionToMultirotorTypeConfigured(); + + default: + return false; + } +#else if(currentMixerConfig.automated_switch){ if ((required_action == MIXERAT_REQUEST_RTH) && STATE(MULTIROTOR)) { @@ -143,14 +469,81 @@ 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) { + abortTransition(false); + return true; + } + switch (mixerProfileAT.phase) { + case MIXERAT_PHASE_IDLE: + //check if mixerAT is required + if (checkMixerATRequired(required_action)) { + mixerProfileAT.request = required_action; + mixerProfileAT.direction = directionForRequest(required_action); + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITION_INITIALIZE; + reprocessState = true; + } else { + resetTransitionScales(); + } + break; + case MIXERAT_PHASE_TRANSITION_INITIALIZE: + mixerProfileAT.request = required_action; + mixerProfileAT.direction = directionForRequest(required_action); + setMixerProfileAT(); + mixerProfileAT.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); + return true; + } + + if (mixerATReadyForHotSwitch(mixerProfileAT.request)) { + isMixerTransitionMixing_requested = false; + mixerProfileAT.progress = 1.0f; + updateTransitionScales(); + if (!outputProfileHotSwitch(nextMixerProfileIndex)) { + abortTransition(false); + return true; + } + mixerProfileAT.hotSwitchDone = true; + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + mixerProfileAT.request = MIXERAT_REQUEST_NONE; + mixerProfileAT.direction = MIXERAT_DIRECTION_NONE; + reprocessState = true; + } else if (mixerProfileAT.usedAirspeed && + currentMixerConfig.vtolTransitionAirspeedTimeoutMs > 0 && + (millis() - mixerProfileAT.transitionStartTime) >= currentMixerConfig.vtolTransitionAirspeedTimeoutMs) { + abortTransition(true); + return true; + } + + updateTransitionScales(); + return false; + break; + 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 +559,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 +580,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) } while (reprocessState); return true; +#endif } bool checkMixerProfileHotSwitchAvalibility(void) @@ -202,7 +595,157 @@ 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 bool missionActive = (navGetCurrentStateFlags() & NAV_AUTO_WP) != 0; + const bool manualControllerConfigured = currentMixerConfig.manualVtolTransitionController && !missionActive; + bool manualControllerEnabled = manualControllerConfigured || manualTransitionSessionLatched; + const bool transitionControllerOwnsProfileSwitch = manualControllerEnabled && transitionModeActive; + const bool mixerProfileModePresent = isModeActivationConditionPresent(BOXMIXERPROFILE); + const int requestedProfileIndex = IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1; + const bool requestedMultirotorProfile = mixerProfileModePresent && + isMultirotorTypePlatform(mixerConfigByIndex(requestedProfileIndex)->platformType); + // If low-speed protection already moved the model back to MC, keep direct + // switching from forcing FW again until the pilot makes a new manual choice. + const bool fwToMcProtectionOwnsProfileSwitch = manualFwToMcProtectionLatched && + STATE(MULTIROTOR) && + !requestedMultirotorProfile; + + if (manualControllerConfigured && transitionModeRisingEdge) { + manualTransitionSessionLatched = true; + } + + if (transitionModeRisingEdge) { + manualFwToMcProtectionLatched = false; + } + + if (transitionModeFallingEdge) { + manualTransitionSessionLatched = false; + } + + if (requestedMultirotorProfile || (!mixerAT_inuse && !STATE(MULTIROTOR))) { + manualFwToMcProtectionLatched = false; + } + + if (mixerAT_inuse && (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating())) { + abortTransition(false); + manualTransitionSessionLatched = false; + manualFwToMcProtectionLatched = false; + mixerAT_inuse = false; + } + + if (!FLIGHT_MODE(FAILSAFE_MODE) && !mixerAT_inuse) + { + if (mixerProfileModePresent && !transitionControllerOwnsProfileSwitch && !fwToMcProtectionOwnsProfileSwitch) { + outputProfileHotSwitch(requestedProfileIndex); + } + } + + // Recompute after a potential direct profile hot-switch because this flag is per-mixer-profile. + manualControllerEnabled = (currentMixerConfig.manualVtolTransitionController && !missionActive) || manualTransitionSessionLatched; + + if (!mixerAT_inuse && + shouldRequestManualFwToMcProtection(manualControllerEnabled) && + checkMixerATRequired(MIXERAT_REQUEST_MANUAL_TO_MC)) { + mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_MC); + mixerAT_inuse = mixerATIsActive(); + if (mixerAT_inuse || STATE(MULTIROTOR)) { + manualFwToMcProtectionLatched = true; + } + } + + if (!manualControllerEnabled) { + // Backward-compatible manual path: level-controlled transition mixing request. + if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) { + isMixerTransitionMixing_requested = transitionModeActive; + if (isMixerTransitionMixing_requested) { + setLegacyTransitionScales(); + } + } + manualTransitionReadyForEdge = true; + } else { + if (!transitionModeActive) { + manualTransitionSessionLatched = false; + manualTransitionReadyForEdge = true; + if (!mixerAT_inuse) { + isMixerTransitionMixing_requested = false; + } + } else if (transitionModeRisingEdge && manualTransitionReadyForEdge && manualTransitionAllowed && !mixerAT_inuse) { + manualTransitionReadyForEdge = false; + if (STATE(MULTIROTOR)) { + mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_FW); + } else if (STATE(AIRPLANE)) { + mixerATUpdateState(MIXERAT_REQUEST_MANUAL_TO_MC); + } + mixerAT_inuse = mixerATIsActive(); + } + + if (!transitionModeActive && + mixerAT_inuse && + !mixerProfileAT.hotSwitchDone && + (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { + abortTransition(false); + mixerAT_inuse = false; + } + + if (mixerAT_inuse && + (mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_FW || mixerProfileAT.request == MIXERAT_REQUEST_MANUAL_TO_MC)) { + mixerATUpdateState(mixerProfileAT.request); + mixerAT_inuse = mixerATIsActive(); + } + } + + manualTransitionModeWasActive = transitionModeActive; + + isMixerTransitionMixing = isMixerTransitionMixing_requested && + ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); + + const uint32_t transitionDebugFlags = + ((uint32_t)mixerProfileAT.direction & 0x3U) | + (mixerATIsActive() ? 1U << 2 : 0U) | + (isMixerTransitionMixing ? 1U << 3 : 0U) | + (transitionModeActive ? 1U << 4 : 0U) | + (mixerProfileAT.usedAirspeed ? 1U << 5 : 0U) | + (mixerProfileAT.hotSwitchDone ? 1U << 6 : 0U) | + (mixerProfileAT.aborted ? 1U << 7 : 0U) | + (currentMixerConfig.manualVtolTransitionController ? 1U << 8 : 0U) | + (currentMixerConfig.vtolTransitionDynamicMixer ? 1U << 9 : 0U) | + (((uint32_t)currentMixerProfileIndex & 0x3U) << 10) | + (((uint32_t)nextMixerProfileIndex & 0x3U) << 12) | + (manualTransitionAllowed ? 1U << 14 : 0U) | + (missionActive ? 1U << 15 : 0U) | + (isMixerTransitionMixing_requested ? 1U << 16 : 0U) | + (FLIGHT_MODE(FAILSAFE_MODE) ? 1U << 17 : 0U) | + (manualControllerEnabled ? 1U << 18 : 0U) | + (IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) ? 1U << 19 : 0U) | + (manualTransitionSessionLatched ? 1U << 20 : 0U); + + // VTOL transition debug channels (DEBUG_VTOL_TRANSITION): + // [0] phase, [1] request, [2] packed transition flags, [3] progress x1000, + // [4] pusherScale x1000, [5] liftScale x1000, [6] mcAuthorityScale x1000, + // [7] transition_PID_mmix_multiplier_pitch from currentMixerConfig + DEBUG_SET(DEBUG_VTOL_TRANSITION, 0, mixerProfileAT.phase); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 1, mixerProfileAT.request); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 2, (int32_t)transitionDebugFlags); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 3, lrintf(constrainf(mixerProfileAT.progress, 0.0f, 1.0f) * 1000.0f)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 4, lrintf(constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f) * 1000.0f)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 5, lrintf(constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f) * 1000.0f)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 6, lrintf(constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f) * 1000.0f)); + DEBUG_SET(DEBUG_VTOL_TRANSITION, 7, currentMixerConfig.transition_PID_mmix_multiplier_pitch); + + if (!isMixerTransitionMixing) { + resetTransitionScales(); + } +#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 +756,103 @@ 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 mixerATIsActive(void) +{ + return mixerProfileAT.phase != MIXERAT_PHASE_IDLE; +} + +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 mixerATGetPusherScale(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.pusherScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif +} + +float mixerATGetLiftScale(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.liftScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif +} + +float mixerATGetMcAuthorityScale(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.mcAuthorityScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif +} + +float mixerATGetFwAuthorityScale(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.fwAuthorityScale, 0.0f, 1.0f); +#else + return 1.0f; +#endif +} + +float mixerATGetBlendToFw(void) +{ +#ifdef USE_AUTO_TRANSITION + return constrainf(mixerProfileAT.blendToFw, 0.0f, 1.0f); +#else + return 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 (currentMixerConfig.manualVtolTransitionController || manualTransitionSessionLatched) { + 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..05a4a9d1bc8 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,72 @@ 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, +#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; +#endif + //mixerProfile Automated Transition PHASE typedef enum { MIXERAT_PHASE_IDLE, MIXERAT_PHASE_TRANSITION_INITIALIZE, MIXERAT_PHASE_TRANSITIONING, +#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 transitionStartAirspeedCmS; + float blendToFw; + float pusherScale; + float liftScale; + float mcAuthorityScale; + float fwAuthorityScale; + timeMs_t transitionStartTime; +#else bool transitionInputMixing; timeMs_t transitionStartTime; timeMs_t transitionStabEndTime; timeMs_t transitionTransEndTime; +#endif } mixerProfileAT_t; extern mixerProfileAT_t mixerProfileAT; bool checkMixerATRequired(mixerProfileATRequest_e required_action); bool mixerATUpdateState(mixerProfileATRequest_e required_action); +bool mixerATIsActive(void); +bool mixerATWasAborted(void); +bool mixerATWasAbortedByAirspeedTimeout(void); +float mixerATGetPusherScale(void); +float mixerATGetLiftScale(void); +float mixerATGetMcAuthorityScale(void); +float mixerATGetFwAuthorityScale(void); +float mixerATGetBlendToFw(void); +bool isMixerProfile2ModeReportedActive(void); +bool isMixerTransitionModeReportedActive(void); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; @@ -81,4 +132,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/pid.c b/src/main/flight/pid.c index a8168fa6081..6ed1cae44ef 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -122,6 +122,35 @@ typedef struct { uint16_t pidSumLimit; } pidState_t; +#ifdef USE_AUTO_TRANSITION +typedef struct { + float kP; + float kI; + float kD; + float kFF; + float gyroRate; + float rateTarget; + float errorGyroIf; + float errorGyroIfLimit; + pt1Filter_t angleFilterState; + rateLimitFilter_t axisAccelFilter; + pt1Filter_t ptermLpfState; + filter_t dtermLpfState; + float previousRateTarget; + float previousRateGyro; +#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 +160,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 +173,18 @@ 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; +#endif #ifdef USE_ANTIGRAVITY static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf; @@ -184,6 +226,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 +463,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 +487,613 @@ float getAxisIterm(uint8_t axis) return pidState[axis].errorGyroIf; } +#ifdef USE_AUTO_TRANSITION +static void resetAutoTransitionTargetPidState(void) +{ + memset(autoTransitionTargetPidState, 0, sizeof(autoTransitionTargetPidState)); + memset(autoTransitionTargetAxisPID, 0, sizeof(autoTransitionTargetAxisPID)); + memset(&autoTransitionTargetHeadingHoldRateFilter, 0, sizeof(autoTransitionTargetHeadingHoldRateFilter)); + memset(&autoTransitionTargetTpaFilter, 0, sizeof(autoTransitionTargetTpaFilter)); + autoTransitionTargetControlProfileIndex = -1; + autoTransitionTargetDTermLpfFilterApplyFn = (filterApplyFnPtr)nullFilterApply; + autoTransitionTargetYawLpfHz = 0; +} + +static bool isAutoTransitionTargetPidActive(void) +{ + return isMixerTransitionMixing && + mixerATIsActive() && + mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW && + isMultirotorTypePlatform(currentMixerConfig.platformType) && + nextMixerProfileIndex >= 0 && + nextMixerProfileIndex < MAX_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 uint32_t refreshRate = getLooptime(); + + resetAutoTransitionTargetPidState(); + + autoTransitionTargetControlProfileIndex = profileIndex; + 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, targetPidProfile->bank_fw.pid[PID_LEVEL].I); + + 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 void updateAutoTransitionTargetPIDCoefficients(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile) +{ + float tpaFactor = 1.0f; + float iTermFactor = 1.0f; + + if (controlProfile->throttle.apa_pow > 0 && + pitotGetValidForAirspeed()) { + tpaFactor = calculateAutoTransitionTargetAirspeedTPAFactor(targetPidProfile, controlProfile); + iTermFactor = calculateAutoTransitionTargetAirspeedITermFactor(targetPidProfile, controlProfile); + } else { + tpaFactor = calculateAutoTransitionTargetTPAFactor(controlProfile); + iTermFactor = tpaFactor; + } + + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + autoTransitionTargetPidState[axis].kP = targetPidProfile->bank_fw.pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; + autoTransitionTargetPidState[axis].kI = targetPidProfile->bank_fw.pid[axis].I / FP_PID_RATE_I_MULTIPLIER * iTermFactor; + autoTransitionTargetPidState[axis].kD = targetPidProfile->bank_fw.pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor; + autoTransitionTargetPidState[axis].kFF = targetPidProfile->bank_fw.pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; + } +} + +static float calcAutoTransitionTargetHorizonRateMagnitude(const pidProfile_t *targetPidProfile) +{ + 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 float modeTransitionStickPos = constrain(targetPidProfile->bank_fw.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, float dT) +{ + int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget; + + if (error > 180) { + error -= 360; + } else if (error < -180) { + error += 360; + } + + float headingHoldRate = error * targetPidProfile->bank_fw.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, flight_dynamics_index_t axis) +{ + float angleTarget; + +#ifdef USE_PROGRAMMING_FRAMEWORK + angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), targetPidProfile->max_angle_inclination[axis]); +#else + angleTarget = pidRcCommandToAngle(rcCommand[axis], targetPidProfile->max_angle_inclination[axis]); +#endif + + if (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, + flight_dynamics_index_t axis, + float horizonRateMagnitude, + float dT) +{ + float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); + + if (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 * (targetPidProfile->bank_fw.pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), + -controlProfile->stabilized.rates[axis] * 10.0f, + controlProfile->stabilized.rates[axis] * 10.0f + ); + + if (targetPidProfile->bank_fw.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, + float bankAngleTarget, + float pitchAngleTarget) +{ + fpVector3_t targetRates; + targetRates.x = 0.0f; + targetRates.y = 0.0f; + + 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); + + 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 + ); + pidState[YAW].rateTarget = constrainf( + pidState[YAW].rateTarget + targetRates.z * targetPidProfile->fixedWingCoordinatedYawGain, + -controlProfile->stabilized.rates[YAW] * 10.0f, + controlProfile->stabilized.rates[YAW] * 10.0f + ); +} + +static void updateAutoTransitionTargetRateTargets(const controlConfig_t *controlProfile, const pidProfile_t *targetPidProfile, 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, 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) : 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, axis)); + + if (STATE(TAILSITTER) && isMixerTransitionMixing && axis == FD_PITCH) { + angleTarget += DEGREES_TO_DECIDEGREES(45); + } + + // Preview the target fixed-wing angle controller using the current transition stick command. + // Airplane 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, 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, 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 void 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); + + if (autoTransitionTargetControlProfileIndex != profileIndex) { + initAutoTransitionTargetPidState(profileIndex, controlProfile, targetPidProfile); + } + + updateAutoTransitionTargetPIDCoefficients(controlProfile, targetPidProfile); + updateAutoTransitionTargetRateTargets(controlProfile, targetPidProfile, dT); + + const float dT_inv = 1.0f / dT; + + 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 + + const bool itermLimitActive = checkAutoTransitionTargetItermLimitingActive(); + const bool itermFreezeActive = checkAutoTransitionTargetItermFreezingActive(targetPidProfile, axis); + + autoTransitionTargetAxisPID[axis] = applyAutoTransitionTargetFixedWingRateController(&autoTransitionTargetPidState[axis], controlProfile, targetPidProfile, axis, itermLimitActive, itermFreezeActive, dT, dT_inv); + } +} + +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 (FLIGHT_MODE(MANUAL_MODE)) { + return lrintf(rcCommand[axis] * scale); + } + + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_FW) { + return lrintf(autoTransitionTargetAxisPID[axis] * scale); + } + + if (mixerProfileAT.direction == MIXERAT_DIRECTION_TO_MC) { + return lrintf(axisPID[axis] * scale); + } + + return 0; +} +#endif + static float FAST_CODE pidRcCommandToAngle(int16_t stick, int16_t maxInclination) { stick = constrain(stick, -500, 500); @@ -1251,6 +1910,9 @@ void FAST_CODE pidController(float dT) pidLoopNowMs = millis(); if (!pidFiltersConfigured) { +#ifdef USE_AUTO_TRANSITION + resetAutoTransitionTargetPidState(); +#endif return; } @@ -1351,6 +2013,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 +2116,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..c61a17f9405 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -188,7 +188,13 @@ 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); +#endif void pidInit(void); bool pidInitFilters(void); @@ -228,4 +234,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..a1e68b7ea0a 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,22 @@ 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 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 +416,26 @@ void servoMixer(float dT) } } +#ifdef USE_AUTO_TRANSITION + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_ROLL] = getAutoTransitionTargetStabilizedInput(FD_ROLL); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_PITCH] = getAutoTransitionTargetStabilizedInput(FD_PITCH); + input[INPUT_AUTOTRANSITION_TARGET_STABILIZED_YAW] = getAutoTransitionTargetStabilizedInput(FD_YAW); +#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 +461,7 @@ void servoMixer(float dT) input[INPUT_STABILIZED_THROTTLE] = mixerThrottleCommand - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] - input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing * 500; //fixed value + input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing ? lrintf(mixerATGetBlendToFw() * 500.0f) : 0; input[INPUT_MIXER_SWITCH_HELPER] = 0; // no input, used to apply speed limit filter from previous servo rules // center the RC input value around the RC middle value @@ -458,6 +562,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 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/navigation/navigation.c b/src/main/navigation/navigation.c index d640e2b25c3..5a064663bed 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -61,6 +61,7 @@ #include "rx/rx.h" #include "sensors/sensors.h" +#include "sensors/pitotmeter.h" #include "sensors/acceleration.h" #include "sensors/boardalignment.h" #include "sensors/battery.h" @@ -83,6 +84,16 @@ #define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec) #define FW_LAND_LOITER_ALT_TOLERANCE 150 +#ifdef USE_AUTO_TRANSITION +// One-shot MC->FW mission retry after airspeed-timeout: 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 +#endif + /*----------------------------------------------------------- * Compatibility for home position *-----------------------------------------------------------*/ @@ -119,7 +130,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 +164,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 @@ -270,6 +292,50 @@ 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_START, + NAV_MISSION_VTOL_TRANSITION_REJECT, +} navMissionVtolTransitionDisposition_e; + +static navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; +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,6 +354,9 @@ 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); @@ -295,6 +364,16 @@ 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); @@ -1044,8 +1123,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 +1345,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 +1389,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState { UNUSED(previousState); +#ifdef USE_AUTO_TRANSITION + navMixerATPendingState = NAV_STATE_IDLE; + clearMissionVTOLTransitionState(); +#endif resetAltitudeController(false); resetHeadingController(); resetPositionController(); @@ -1965,6 +2076,344 @@ 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); +} + +#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 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; +} + +static navigationFSMEvent_t getMcToFwTransitionFailEvent(const mixerProfileATRequest_e request) +{ + switch ((navVtolTransitionFailActionMcToFw_e)navConfig()->general.vtol_transition_fail_action_mc_to_fw) { + case NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_POSH: + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + case NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_RTH: + return request == MIXERAT_REQUEST_RTH ? NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME : NAV_FSM_EVENT_SWITCH_TO_RTH; + case NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_EMERGENCY_LANDING: + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + case NAV_VTOL_TRANSITION_FAIL_ACTION_MC_TO_FW_IDLE: + default: + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } +} + +static navigationFSMEvent_t getFwToMcTransitionFailEvent(const mixerProfileATRequest_e request) +{ + UNUSED(request); + + switch ((navVtolTransitionFailActionFwToMc_e)navConfig()->general.vtol_transition_fail_action_fw_to_mc) { + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_RTH: + return NAV_FSM_EVENT_SWITCH_TO_RTH; + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_EMERGENCY_LANDING: + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_LOITER: + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_FORCE_SWITCH: + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + case NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_IDLE: + default: + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } +} + +static navigationFSMEvent_t getTransitionFailEvent(const mixerProfileATRequest_e request) +{ + if (isTransitionRetryToFixedWingRequest(request)) { + return getMcToFwTransitionFailEvent(request); + } + + if (isTransitionToMultirotorRequest(request)) { + return getFwToMcTransitionFailEvent(request); + } + + return NAV_FSM_EVENT_SWITCH_TO_IDLE; +} + +static bool tryForceSwitchAfterFwToMcFailure(const mixerProfileATRequest_e request) +{ + if (!isTransitionToMultirotorRequest(request)) { + return false; + } + + if ((navVtolTransitionFailActionFwToMc_e)navConfig()->general.vtol_transition_fail_action_fw_to_mc != NAV_VTOL_TRANSITION_FAIL_ACTION_FW_TO_MC_FORCE_SWITCH) { + return false; + } + + return STATE(AIRPLANE) && outputProfileHotSwitch(nextMixerProfileIndex); +} + +static void clearMissionVTOLTransitionState(void) +{ + navMixerATMissionTransition.active = false; + navMixerATMissionTransition.request = MIXERAT_REQUEST_NONE; + navMixerATMissionTransition.heading = posControl.actualState.yaw; + navMixerATMissionTransition.retryAttempted = false; + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + navMixerATMissionTransition.retryScanStartHeading = posControl.actualState.yaw; + navMixerATMissionTransition.retryTargetHeading = posControl.actualState.yaw; + navMixerATMissionTransition.retryBestHeading = posControl.actualState.yaw; + navMixerATMissionTransition.retryScannedCd = 0; + navMixerATMissionTransition.retryBestAirspeedCmS = -1.0f; + navMixerATMissionTransition.retryHadTrustedAirspeedSample = false; + navMixerATMissionTransition.retryStartTimeMs = 0; + navMixerATMissionTransition.retryStepStartTimeMs = 0; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; +} + +static void beginMissionTransitionRetryScan(const mixerProfileATRequest_e request) +{ + navMixerATMissionTransition.request = request; + navMixerATMissionTransition.retryAttempted = true; + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_SCAN; + navMixerATMissionTransition.retryScanStartHeading = wrap_36000(posControl.actualState.yaw); + navMixerATMissionTransition.retryTargetHeading = navMixerATMissionTransition.retryScanStartHeading; + navMixerATMissionTransition.retryBestHeading = navMixerATMissionTransition.retryScanStartHeading; + navMixerATMissionTransition.retryScannedCd = 0; + navMixerATMissionTransition.retryBestAirspeedCmS = -1.0f; + navMixerATMissionTransition.retryHadTrustedAirspeedSample = false; + navMixerATMissionTransition.retryStartTimeMs = millis(); + navMixerATMissionTransition.retryStepStartTimeMs = navMixerATMissionTransition.retryStartTimeMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + navMixerATMissionTransition.retryHoldPos = navGetCurrentActualPositionAndVelocity()->pos; +} + +static navMixerATRetryScanResult_e updateMissionTransitionRetryScan(void) +{ + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_IDLE) { + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + const timeMs_t nowMs = millis(); + const bool overallTimedOut = (nowMs - navMixerATMissionTransition.retryStartTimeMs) >= NAV_MIXERAT_RETRY_MAX_TOTAL_MS; + if (overallTimedOut) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_FAILED; + } + + const int32_t headingError = ABS(wrap_18000(navMixerATMissionTransition.retryTargetHeading - posControl.actualState.yaw)); + const bool headingReached = headingError <= NAV_MIXERAT_RETRY_HEADING_TOL_CD; + const bool stepTimedOut = (nowMs - navMixerATMissionTransition.retryStepStartTimeMs) >= NAV_MIXERAT_RETRY_HEADING_STEP_TIMEOUT_MS; + + if (headingReached) { + if (!navMixerATMissionTransition.retryHeadingReachedTimeMs) { + navMixerATMissionTransition.retryHeadingReachedTimeMs = nowMs; + } + } else { + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + } + + if (!headingReached && !stepTimedOut) { + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + if (headingReached && + !stepTimedOut && + (nowMs - navMixerATMissionTransition.retryHeadingReachedTimeMs) < NAV_MIXERAT_RETRY_HEADING_SETTLE_MS) { + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_SCAN) { + float airspeedCmS = 0.0f; + if (hasTrustedPitotAirspeed(&airspeedCmS)) { + navMixerATMissionTransition.retryHadTrustedAirspeedSample = true; + if (airspeedCmS > navMixerATMissionTransition.retryBestAirspeedCmS) { + navMixerATMissionTransition.retryBestAirspeedCmS = airspeedCmS; + navMixerATMissionTransition.retryBestHeading = wrap_36000(posControl.actualState.yaw); + } + } + + navMixerATMissionTransition.retryScannedCd += NAV_MIXERAT_RETRY_SCAN_STEP_CD; + if (navMixerATMissionTransition.retryScannedCd >= DEGREES_TO_CENTIDEGREES(360)) { + if (!navMixerATMissionTransition.retryHadTrustedAirspeedSample) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_FAILED; + } + + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_ALIGN; + navMixerATMissionTransition.retryTargetHeading = wrap_36000(navMixerATMissionTransition.retryBestHeading); + navMixerATMissionTransition.retryStepStartTimeMs = nowMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + navMixerATMissionTransition.retryTargetHeading = + wrap_36000(navMixerATMissionTransition.retryScanStartHeading + navMixerATMissionTransition.retryScannedCd); + navMixerATMissionTransition.retryStepStartTimeMs = nowMs; + navMixerATMissionTransition.retryHeadingReachedTimeMs = 0; + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; + } + + if (navMixerATMissionTransition.retryStage == NAV_MIXERAT_RETRY_STAGE_ALIGN) { + if (!headingReached) { + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_FAILED; + } + + navMixerATMissionTransition.heading = wrap_36000(navMixerATMissionTransition.retryBestHeading); + navMixerATMissionTransition.retryStage = NAV_MIXERAT_RETRY_STAGE_IDLE; + return NAV_MIXERAT_RETRY_SCAN_READY_TO_RETRY; + } + + return NAV_MIXERAT_RETRY_SCAN_IN_PROGRESS; +} + +static navMissionVtolTransitionDisposition_e prepareMissionVTOLTransition(const navWaypoint_t *waypoint) +{ + const navMissionUserAction_e configuredUserAction = (navMissionUserAction_e)navConfig()->general.vtol_mission_transition_user_action; + const uint16_t configuredUserActionMask = missionUserActionMask(configuredUserAction); + + if (!configuredUserActionMask) { + return NAV_MISSION_VTOL_TRANSITION_NONE; + } + + // The configured USER action bit itself is the absolute target selector: + // 0 -> MC target, 1 -> FW target. + const bool transitionToFixedWing = ((((uint16_t)waypoint->p3) & configuredUserActionMask) != 0); + const mixerProfileATRequest_e requestedAction = transitionToFixedWing ? MIXERAT_REQUEST_MISSION_TO_FW : MIXERAT_REQUEST_MISSION_TO_MC; + + // Idempotent mission command: continue immediately if already in requested platform state. + if ((transitionToFixedWing && STATE(AIRPLANE)) || (!transitionToFixedWing && STATE(MULTIROTOR))) { + return NAV_MISSION_VTOL_TRANSITION_CONTINUE; + } + + if (!ARMING_FLAG(ARMED) || + FLIGHT_MODE(FAILSAFE_MODE) || + areSensorsCalibrating() || + posControl.flags.estPosStatus < EST_USABLE || + posControl.flags.estHeadingStatus < EST_USABLE || + !isModeActivationConditionPresent(BOXMIXERPROFILE) || + !checkMixerProfileHotSwitchAvalibility() || + mixerATIsActive()) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + const uint16_t transitionMinAltitude = navConfig()->general.vtol_mission_transition_min_altitude; + if (transitionMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z < transitionMinAltitude) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + const flyingPlatformType_e targetPlatformType = mixerConfigByIndex(nextMixerProfileIndex)->platformType; + if ((transitionToFixedWing && targetPlatformType != PLATFORM_AIRPLANE) || + (!transitionToFixedWing && !isMissionTransitionToMultirotorType(targetPlatformType))) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + if (!checkMixerATRequired(requestedAction)) { + return NAV_MISSION_VTOL_TRANSITION_REJECT; + } + + int32_t transitionHeading = posControl.actualState.yaw; + if (transitionToFixedWing) { + fpVector3_t nextWpPos; + if (getLocalPosNextWaypoint(&nextWpPos)) { + transitionHeading = calculateBearingToDestination(&nextWpPos); + } + } + + navMixerATMissionTransition.request = requestedAction; + navMixerATMissionTransition.heading = wrap_36000(transitionHeading); + navMixerATMissionTransition.active = true; + return NAV_MISSION_VTOL_TRANSITION_START; +} + +static void updateMissionTransitionGuidance(void) +{ + if (navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE && + isTransitionRetryToFixedWingRequest(navMixerATMissionTransition.request) && + STATE(MULTIROTOR)) { + setDesiredPosition(&navMixerATMissionTransition.retryHoldPos, + navMixerATMissionTransition.retryTargetHeading, + NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return; + } + + if (navMixerATMissionTransition.active && + navMixerATMissionTransition.request == MIXERAT_REQUEST_MISSION_TO_FW && + STATE(MULTIROTOR)) { + fpVector3_t transitionTarget; + calculateFarAwayTarget(&transitionTarget, navMixerATMissionTransition.heading, NAV_MIXERAT_MISSION_TRANSITION_TRACK_DISTANCE_CM); + setDesiredPosition(&transitionTarget, navMixerATMissionTransition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return; + } + + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); +} +#endif + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState) { /* A helper function to do waypoint-specific action */ @@ -1973,12 +2422,26 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_HOLD_TIME: case NAV_WP_ACTION_WAYPOINT: - case NAV_WP_ACTION_LAND: - calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); + case NAV_WP_ACTION_LAND: { + const navWaypoint_t * const activeWaypoint = &posControl.waypointList[posControl.activeWaypointIndex]; + calculateAndSetActiveWaypoint(activeWaypoint); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; posControl.wpAltitudeReached = false; + +#ifdef USE_AUTO_TRANSITION + clearMissionVTOLTransitionState(); + const navMissionVtolTransitionDisposition_e transitionAction = prepareMissionVTOLTransition(activeWaypoint); + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_START) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } + if (transitionAction == NAV_MISSION_VTOL_TRANSITION_REJECT) { + return NAV_FSM_EVENT_ERROR; + } +#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) @@ -2284,7 +2747,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 +2756,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 +2773,108 @@ 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 + if (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE)) { + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + clearMissionVTOLTransitionState(); + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + mixerProfileATRequest_e required_action; + switch (navMixerATPendingState) + { + 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 (navMixerATMissionTransition.retryStage != NAV_MIXERAT_RETRY_STAGE_IDLE) { + const navMixerATRetryScanResult_e retryResult = updateMissionTransitionRetryScan(); + if (retryResult == NAV_MIXERAT_RETRY_SCAN_READY_TO_RETRY) { + mixerATUpdateState(required_action); + } else if (retryResult == NAV_MIXERAT_RETRY_SCAN_FAILED) { + const navigationFSMEvent_t nextEvent = getTransitionFailEvent(required_action); + resetPositionController(); + resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + clearMissionVTOLTransitionState(); + if (nextEvent != NAV_FSM_EVENT_SWITCH_TO_IDLE) { + setupAltitudeController(); + } + return nextEvent; + } + updateMissionTransitionGuidance(); + return NAV_FSM_EVENT_NONE; + } + + if (mixerATUpdateState(required_action)){ + // MixerAT is done, switch to next state + bool transitionAborted = mixerATWasAborted(); + const bool transitionTimeout = mixerATWasAbortedByAirspeedTimeout(); + const bool missionTransitionWasActive = navMixerATMissionTransition.active; + if (transitionAborted && + transitionTimeout && + !navMixerATMissionTransition.retryAttempted && + canRetryTransitionAfterAirspeedTimeout(required_action) && + STATE(MULTIROTOR) && + ((required_action == MIXERAT_REQUEST_MISSION_TO_FW && missionTransitionWasActive) || + required_action == MIXERAT_REQUEST_RTH)) { + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + beginMissionTransitionRetryScan(required_action); + updateMissionTransitionGuidance(); + return NAV_FSM_EVENT_NONE; + } + + if (transitionAborted && tryForceSwitchAfterFwToMcFailure(required_action)) { + transitionAborted = false; + } + + navigationFSMEvent_t nextEvent = NAV_FSM_EVENT_SWITCH_TO_IDLE; + if (transitionAborted) { + nextEvent = getTransitionFailEvent(required_action); + } else { + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LANDING: + nextEvent = NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + break; + case NAV_STATE_WAYPOINT_PRE_ACTION: + if (missionTransitionWasActive) { + nextEvent = NAV_FSM_EVENT_MIXERAT_MISSION_RESUME; + } + break; + default: + break; + } + } + + resetPositionController(); + resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + 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 +2912,16 @@ 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 + clearMissionVTOLTransitionState(); +#endif return NAV_FSM_EVENT_SUCCESS; } @@ -5062,6 +5639,10 @@ void navigationInit(void) { /* Initial state */ posControl.navState = NAV_STATE_IDLE; +#ifdef USE_AUTO_TRANSITION + navMixerATPendingState = NAV_STATE_IDLE; + 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..42f7c7737f0 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 diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 6f076659caf..c4296213f43 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -183,6 +183,9 @@ 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 + 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, 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 -