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), a predicted floor breach engages an automatic upright + climb recovery. Switch the 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] to arm the floor after takeoff and to release the recovery
+
+| 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,66 @@ Waypoint radius [cm]. Waypoint would be considered reached if machine is within
 
 ---
 
+### ohold_hover_thr_d
+
+Hover throttle D gain [throttle us per m/s of climb rate]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 30 | 0 | 100 |
+
+---
+
+### ohold_hover_thr_i
+
+Hover throttle I gain [throttle us per m per second]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 0 | 100 |
+
+---
+
+### ohold_hover_thr_p
+
+Hover throttle P gain [throttle us per m of altitude error] while PROP HANG is held. The hover base throttle is learned online (I-term seeded from the pilot's throttle at engage)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 25 | 0 | 100 |
+
+---
+
+### ohold_inverted_pitch_trim
+
+Pitch trim [deg] on the INVERTED hold target, positive = nose above the horizon. Inverted flight typically needs a few degrees to hold altitude (down-elevator bias)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -15 | 15 |
+
+---
+
+### ohold_knife_left_pitch_trim
+
+Pitch trim [deg] on the KNIFE EDGE LEFT hold target, positive = nose above the horizon, held via the rudder. Separate per side: the body-fixed prop effects (spiral slipstream, torque, P-factor) point to the vertically opposite direction after the 180 deg roll to the other side, so left/right = shared fuselage-lift part +/- prop part. Reversed prop rotation swaps the sides
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -15 | 15 |
+
+---
+
+### ohold_knife_right_pitch_trim
+
+Pitch trim [deg] on the KNIFE EDGE RIGHT hold target, positive = nose above the horizon, held via the rudder. See ohold_knife_left_pitch_trim for why the sides differ
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -15 | 15 |
+
+---
+
 ### opflow_hardware
 
 Selection of OPFLOW hardware.
@@ -6522,6 +6672,26 @@ Turtle mode power factor
 
 ---
 
+### tvc_gain
+
+Overall thrust vectoring deflection gain [%] at full thrust, applied to the TVC servo mixer input sources
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 200 |
+
+---
+
+### 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,23 @@ 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/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/config/parameter_group_ids.h

@@ -132,7 +132,13 @@
 #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_INAV_END PG_HOVER_THROTTLE_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/fc_core.c

@@ -87,6 +87,9 @@
 #include "flight/servos.h"
 #include "flight/pid.h"
 #include "flight/imu.h"
+#include "flight/altitude_floor.h"
+#include "flight/figure_sequencer.h"
+#include "flight/orientation_hold.h"
 #include "flight/rate_dynamics.h"
 
 #include "flight/failsafe.h"
@@ -683,22 +686,46 @@ void processRx(timeUs_t currentTimeUs)
     bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
     bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
 
-    /* Disable stabilised modes initially, will be enabled as required with priority ANGLE > HORIZON > ANGLEHOLD
+    /* Disable stabilised modes initially, will be enabled as required with priority
+     * auto ANGLE (failsafe/nav) > ALT FLOOR recovery > ANGLE > HORIZON > ORIENTATION HOLD > ANGLEHOLD
      * MANUAL mode has priority over these modes except when ANGLE auto enabled */
     DISABLE_FLIGHT_MODE(ANGLE_MODE);
     DISABLE_FLIGHT_MODE(HORIZON_MODE);
     DISABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
+#ifdef USE_ORIENTATION_HOLD
+    DISABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE);
+    altitudeFloorUpdate();
+    figureSequencerUpdate();
+#endif
 
     if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) {
-        if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) {
+        if (autoEnableAngle) {
+            ENABLE_FLIGHT_MODE(ANGLE_MODE);
+#ifdef USE_ORIENTATION_HOLD
+        } else if (STATE(AIRPLANE) && altitudeFloorRecoveryActive()) {
+            // Automatic floor recovery: upright + climb, overrides the pilot's
+            // stabilised mode selection until back above the floor
+            ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE);
+#endif
+        } else if (IS_RC_MODE_ACTIVE(BOXANGLE)) {
             ENABLE_FLIGHT_MODE(ANGLE_MODE);
         } else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) {
             ENABLE_FLIGHT_MODE(HORIZON_MODE);
+#ifdef USE_ORIENTATION_HOLD
+        } else if (STATE(AIRPLANE) && orientationHoldIsRequested()) {
+            ENABLE_FLIGHT_MODE(ORIENTATION_HOLD_MODE);
+#endif
         } else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
             ENABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
         }
     }
 
+#ifdef USE_ORIENTATION_HOLD
+    if (!FLIGHT_MODE(ORIENTATION_HOLD_MODE)) {
+        orientationHoldResetSourceTracking();
+    }
+#endif
+
     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
         LED1_ON;
     } else {