Fixed wing: quaternion orientation hold — inverted flight, knife edge, prop hang, figure sequencer (RFC, testers wanted)Feature/quaternion attitude hold

docs/Settings.md

@@ -392,6 +392,36 @@ Optical flow module alignment (default CW0_DEG_FLIP)
 
 ---
 
+### alt_floor_altitude
+
+Altitude floor [m above home]. With the ALT FLOOR mode active and armed (climbed above floor + margin once), SINKING THROUGH the floor engages an automatic upright + climb recovery - no prediction, the crossing is the trigger. Set the floor high enough that the recovery fits below it (a dive recovery consumes roughly 15-25 m). Back at the floor the aircraft ORBITS the breach point on the fixed-wing loiter (nav_fw_loiter_radius) and waits - GPS-anchored while the position estimate is healthy (level flight restores the antenna's sky view), a constant-bank circle otherwise; the pilot gets time to collect themselves, there is no automatic hand-back. SET nav_fw_loiter_radius TO MATCH YOUR SPEED: the circle must be physically flyable, radius >= v^2 / (9.81 * tan(bank)) - an aerobatic airframe at 25 m/s needs roughly 150 m; too small a radius makes the loiter hunt at full bank. THE PILOT OVERRIDES THE AUTOPILOT: held sticks keep steering (a full held rudder drives a spin straight through the floor) - release the sticks and the floor catches; centering the sticks once and then deflecting roll/pitch takes over and releases the orbit. A catch that interrupts ANY active aerobatic mode (every hold, every figure, the sequencer) LATCHES that mode out until the pilot switches it away. Switch the ALT FLOOR mode off to land.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 30 | 5 | 500 |
+
+---
+
+### alt_floor_climb_pitch
+
+Nose up pitch target [deg] flown during altitude floor recovery
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 15 | 5 | 45 |
+
+---
+
+### alt_floor_margin
+
+Margin above the altitude floor [m]: the floor arms after climbing above floor + margin once, and the recovery climb ends there - back above floor + margin and climbing, control returns to the pilot. A roll/pitch input after the catch (sticks centered once first) releases the recovery immediately.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 2 | 100 |
+
+---
+
 ### alt_hold_deadband
 
 Defines the deadband of throttle during alt_hold [r/c points]
@@ -1162,6 +1192,66 @@ If failsafe activated when throttle is low for this much time - bypass failsafe
 
 ---
 
+### fig_assist_max
+
+Cap [deg] on the altitude assist nose-up offset
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 12 | 0 | 30 |
+
+---
+
+### fig_assist_vz_gain
+
+Altitude assist: nose-up offset [deg per m/s] of sink rate during figures. Keep low: the climb rate estimate lags and a strong damping term fights fast figures
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1 | 0 | 20 |
+
+---
+
+### fig_assist_z_gain
+
+Altitude assist: nose-up offset [deg per 10 m] of altitude error during figures. The controller distributes the offset to elevator and rudder as the roll phase demands
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 20 | 0 | 100 |
+
+---
+
+### fig_loop_rate
+
+Pitch rate [deg/s] flown by the FIGURE LOOP mode
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 90 | 30 | 360 |
+
+---
+
+### fig_point_dwell
+
+Dwell time [ms] on each point of the FIGURE 4PT ROLL
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 500 | 100 | 2000 |
+
+---
+
+### fig_roll_rate
+
+Roll rate [deg/s] flown by the FIGURE ROLL and FIGURE 4PT ROLL modes
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 90 | 30 | 360 |
+
+---
+
 ### fixed_wing_auto_arm
 
 Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured.
@@ -4462,6 +4552,46 @@ Waypoint radius [cm]. Waypoint would be considered reached if machine is within
 
 ---
 
+### ohold_entry_rate
+
+Target slew rate [deg/s] for entering an orientation hold preset (INVERTED, KNIFE EDGE, PROP HANG). The entry rolls the hold target from the current attitude to the preset at this rate; figures keep their own fig_roll_rate / fig_loop_rate
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 180 | 30 | 720 |
+
+---
+
+### ohold_load_limit
+
+Load budget [g x 10] the governor holds figures and spins to - a fact about the airframe (what it may pull), not a tuning knob. Load is speed times rotation rate, so at a given speed the budget is simultaneously the fastest rotation and the tightest radius (r = v^2/a): the governor slows the commanded rotation and the target slew with the measured overload, and bleeds throttle while a figure or spin flies (a governed rotation at full power just converts into speed, the load would stay). Plain holds at 1 g are untouched. 0 disables the governor.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 40 | 0 | 160 |
+
+---
+
+### ohold_stick_angle
+
+Body-frame target offset [deg] at full roll/pitch stick while an orientation hold preset is active: the deflection is a held angle offset from the rotated reference (carving), centered sticks return the target at ohold_stick_return_rate. Yaw stays a rate command. 0 = sticks act as raw rate commands like before
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 30 | 0 | 90 |
+
+---
+
+### ohold_stick_return_rate
+
+Rate [deg/s] the hold target returns to the preset after the roll/pitch sticks center
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 45 | 5 | 180 |
+
+---
+
 ### opflow_hardware
 
 Selection of OPFLOW hardware.
@@ -5862,6 +5992,46 @@ Defines rotation rate on ROLL axis that UAV will try to archive on max. stick de
 
 ---
 
+### rotor_guard_bank
+
+Autogyro tip-over guard (ROTOR GUARD mode): bank angle [deg] beyond which, while sinking, the roll excursion counts as a tip-over (rotor rpm decayed, lateral tilt authority gone). Recovery: wings level, nose slightly down, throttle floor - thrust is the only lever that restores rotor rpm. TUNE PER AIRFRAME to just above the steepest bank it flies on purpose; the default is deliberately conservative, the SITL-proven Durafly Auto-G2 value is 45.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 60 | 30 | 90 |
+
+---
+
+### rotor_guard_pitch
+
+Pitch target [deg] during rotor guard recovery, negative = nose down: feeds the disk (inflow -> rotor rpm -> authority)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| -5 | -20 | 10 |
+
+---
+
+### rotor_guard_sink
+
+Minimum sink rate [cm/s] for the tip-over detection - a banked climb or a flown figure does not trip the guard
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 10 | 1000 |
+
+---
+
+### rotor_guard_throttle_add
+
+Recovery throttle floor = cruise throttle + this [us]. More pilot throttle always wins, and an IDLE throttle stick disables the guard entirely (landing intent - the guard never spins the thrust up against a deliberate throttle-zero; pulling to idle releases a running recovery). Must be enough that the airframe LEVELS OFF at the recovery attitude - a T/W below 1 needs a fatter floor (the SITL-proven Auto-G2 value is 380; the default merely arrests the roll, not the sink).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 250 | 0 | 800 |
+
+---
+
 ### rpm_gyro_filter_enabled
 
 Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV
@@ -6242,6 +6412,76 @@ The strength factor of a Smith Predictor of PID measurement. In percents
 
 ---
 
+### soar_alt_max
+
+Leave the thermal once this altitude [m] is reached.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 500 | 0 | 5000 |
+
+---
+
+### soar_alt_min
+
+Do not enter a thermal below this altitude [m] - a safety floor for autonomous soaring.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 50 | 0 | 3000 |
+
+---
+
+### soar_bank
+
+Bank angle [deg] flown while circling a thermal.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 35 | 15 | 50 |
+
+---
+
+### soar_centre_gain
+
+Gain [%] on the thermal-centering gradient shift. Higher centres faster but chases turbulence; the wind-drift shift is always applied at full wind speed regardless.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 300 |
+
+---
+
+### soar_sink_level
+
+Level-flight sink rate [cm/s] at the tuning airspeed, used to compensate the aircraft's own sink out of the net vario. Raise it if the vario reads high in still air, lower it if it reads low. Default ~60 is a 1.8 m motor glider (measured min sink 0.6 m/s); a clean sailplane is lower.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 60 | 0 | 500 |
+
+---
+
+### soar_vario_exit
+
+Net climb rate [cm/s] below which circling stops and the aircraft returns to cruise (the thermal was flown through or died).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 1000 |
+
+---
+
+### soar_vario_trigger
+
+Net (total-energy) climb rate [cm/s] above which the SOARING mode stops cruising and starts circling a thermal. Requires a pitot - the net vario is meaningless without airspeed.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 50 | 0 | 1000 |
+
+---
+
 ### spektrum_sat_bind
 
 0 = disabled. Used to bind the spektrum satellite to RX
@@ -6522,6 +6762,26 @@ Turtle mode power factor
 
 ---
 
+### tvc_gain
+
+Overall thrust vectoring deflection gain [%] at full thrust, applied to the TVC servo mixer input sources. Values above 100 use more of the mechanical vectoring travel per stabilized unit (the output stays clamped at full deflection); hover-heavy setups typically need 200 or more.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 400 |
+
+---
+
+### tvc_thrust_comp
+
+Inverse thrust compensation [%] for the TVC inputs: vane/tilt authority scales with thrust, 100 compensates fully (deflection ~ 1/thrust, capped at low thrust), 0 disables
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 100 |
+
+---
+
 ### tz_automatic_dst
 
 Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`.

src/main/CMakeLists.txt

@@ -323,6 +323,8 @@ main_sources(COMMON_SRC
 
     flight/failsafe.c
     flight/failsafe.h
+    flight/figure_sequencer.c
+    flight/figure_sequencer.h
     flight/imu.c
     flight/imu.h
     flight/kalman.c
@@ -331,15 +333,29 @@ main_sources(COMMON_SRC
     flight/smith_predictor.h
     flight/rate_dynamics.c
     flight/rate_dynamics.h
+    flight/altitude_floor.c
+    flight/altitude_floor.h
+    flight/rotor_guard.c
+    flight/rotor_guard.h
+    flight/soaring.c
+    flight/soaring.h
+    flight/crash_detection.c
+    flight/crash_detection.h
+    flight/hover_throttle.c
+    flight/hover_throttle.h
     flight/mixer.c
     flight/mixer.h
+    flight/orientation_hold.c
+    flight/orientation_hold.h
     flight/pid.c
     flight/pid.h
     flight/pid_autotune.c
     flight/power_limits.c
     flight/power_limits.h
     flight/rth_estimator.c
     flight/rth_estimator.h
+    flight/thrust_vectoring.c
+    flight/thrust_vectoring.h
     flight/servos.c
     flight/servos.h
     flight/mixer_profile.c

src/main/build/debug.h

@@ -79,6 +79,7 @@ typedef enum {
     DEBUG_GPS,
     DEBUG_LULU,
     DEBUG_SBUS2,
+    DEBUG_FW_AEROBATICS,   // 0-3: learned regime gain scale [%] hover/inverted/knife/figure
     DEBUG_COUNT // also update debugModeNames in cli.c
 } debugType_e;
 

src/main/config/parameter_group_ids.h

@@ -132,7 +132,16 @@
 #define PG_GEOZONE_CONFIG 1042
 #define PG_GEOZONES 1043
 #define PG_GEOZONE_VERTICES 1044
-#define PG_INAV_END PG_GEOZONE_VERTICES
+#define PG_ALTITUDE_FLOOR_CONFIG 1045
+#define PG_THRUST_VECTORING_CONFIG 1046
+#define PG_ORIENTATION_HOLD_CONFIG 1047
+#define PG_FIGURE_SEQUENCER_CONFIG 1048
+#define PG_FIGURE_SEQUENCE 1049
+#define PG_HOVER_THROTTLE_CONFIG 1050
+#define PG_CRASH_DETECTION_CONFIG 1051
+#define PG_ROTOR_GUARD_CONFIG 1052
+#define PG_SOARING_CONFIG 1053
+#define PG_INAV_END PG_SOARING_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/accgyro/accgyro_fake.c

@@ -82,7 +82,9 @@ static int16_t fakeAccData[XYZ_AXIS_COUNT];
 
 static void fakeAccInit(accDev_t *acc)
 {
-    acc->acc_1G = 9806;
+    acc->acc_1G = 2048; // 16 G scale, matching a real IMU (HITL injects
+                        // acc.accADCf directly in g, so this only sets the
+                        // full-scale the crash detector reads back)
 }
 
 void fakeAccSet(int16_t x, int16_t y, int16_t z)