diff --git a/docs/Settings.md b/docs/Settings.md
index 3d7351dc371..a0dcdccfa11 100644
--- a/docs/Settings.md
+++ b/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`.
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index c243d9215f3..f584bdcc892 100755
--- a/src/main/CMakeLists.txt
+++ b/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,8 +333,14 @@ 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
@@ -340,6 +348,8 @@ main_sources(COMMON_SRC
     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
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 2acb9c8172e..a5f29774ffa 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/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
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 3223aca497e..2bb000dbcdd 100644
--- a/src/main/fc/fc_core.c
+++ b/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 {
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 3ec3f89f3f0..6ede812094a 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -75,7 +75,9 @@
 #include "fc/settings.h"
 
 #include "flight/failsafe.h"
+#include "flight/figure_sequencer.h"
 #include "flight/imu.h"
+#include "flight/orientation_hold.h"
 #include "flight/mixer_profile.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
@@ -565,6 +567,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU8(dst, 0);
         }
         break;
+#ifdef USE_ORIENTATION_HOLD
+    case MSP2_INAV_FIGURE_SEQUENCE:
+        for (int i = 0; i < MAX_FIGURE_SEQUENCE_SEGMENTS; i++) {
+            sbufWriteU8(dst, figureSequence(i)->type);
+            sbufWriteU16(dst, figureSequence(i)->p1);
+            sbufWriteU16(dst, figureSequence(i)->p2);
+            sbufWriteU16(dst, figureSequence(i)->p3);
+            sbufWriteU8(dst, figureSequence(i)->flags);
+        }
+        break;
+#endif
+
     case MSP2_INAV_SERVO_MIXER:
         for (int i = 0; i < MAX_SERVO_RULES; i++) {
             sbufWriteU8(dst, customServoMixers(i)->targetChannel);
@@ -2371,6 +2385,26 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         } else
             return MSP_RESULT_ERROR;
         break;
+
+#ifdef USE_ORIENTATION_HOLD
+    case MSP2_INAV_SET_FIGURE_SEQUENCE:
+        sbufReadU8Safe(&tmp_u8, src);
+        if ((dataSize == 9) && (tmp_u8 < MAX_FIGURE_SEQUENCE_SEGMENTS)) {
+            figureSegment_t *seg = figureSequenceMutable(tmp_u8);
+            seg->type = sbufReadU8(src);
+            seg->p1 = sbufReadU16(src);
+            seg->p2 = sbufReadU16(src);
+            seg->p3 = sbufReadU16(src);
+            seg->flags = sbufReadU8(src);
+            if (seg->type >= FIGSEG_TYPE_COUNT) {
+                seg->type = FIGSEG_END;
+                return MSP_RESULT_ERROR;
+            }
+        } else
+            return MSP_RESULT_ERROR;
+        break;
+#endif
+
 #ifdef USE_PROGRAMMING_FRAMEWORK
     case MSP2_INAV_SET_LOGIC_CONDITIONS:
         sbufReadU8Safe(&tmp_u8, src);
@@ -4456,6 +4490,42 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         break;
 #endif
 
+#ifdef USE_ORIENTATION_HOLD
+    case MSP2_INAV_ORIENTATION_HOLD_TEST: {
+        // Level-1 test injection (bench/HIL): evaluate the orientation hold
+        // error function and the level gain on the given quaternions.
+        // Pure computation on this MCU's float32 - no controller or
+        // estimator state is touched, safe in any build/flight state.
+        if (dataSize != 8 * sizeof(uint32_t)) {
+            *ret = MSP_RESULT_ERROR;
+            break;
+        }
+        union { uint32_t u; float f; } pun;
+        fpQuaternion_t qEst, qTarget;
+        float * const in[8] = { &qEst.q0, &qEst.q1, &qEst.q2, &qEst.q3,
+                                &qTarget.q0, &qTarget.q1, &qTarget.q2, &qTarget.q3 };
+        for (int i = 0; i < 8; i++) {
+            pun.u = sbufReadU32(src);
+            *in[i] = pun.f;
+        }
+
+        fpVector3_t errDeg;
+        orientationHoldComputeAttitudeError(&errDeg, &qEst, &qTarget);
+        for (int i = 0; i < 3; i++) {
+            pun.f = errDeg.v[i];
+            sbufWriteU32(dst, pun.u);
+        }
+        for (int axis = 0; axis < 3; axis++) {
+            pun.f = constrainf(errDeg.v[axis] * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER),
+                               -currentControlProfile->stabilized.rates[axis] * 10.0f,
+                                currentControlProfile->stabilized.rates[axis] * 10.0f);
+            sbufWriteU32(dst, pun.u);
+        }
+        *ret = MSP_RESULT_ACK;
+        break;
+    }
+#endif
+
     case MSP2_COMMON_SETTING:
         *ret = mspSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR;
         break;
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 65654ccd97b..e86cff8ad36 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -109,6 +109,16 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXGIMBALRLOCK,      .boxName = "GIMBAL LEVEL ROLL", .permanentId = 66 },
     { .boxId = BOXGIMBALCENTER,     .boxName = "GIMBAL CENTER",     .permanentId = 67 },
     { .boxId = BOXGIMBALHTRK,       .boxName = "GIMBAL HEADTRACKER", .permanentId = 68 },
+    { .boxId = BOXINVERTED,         .boxName = "INVERT",          .permanentId = 69 },
+    { .boxId = BOXKNIFELEFT,        .boxName = "KNIFE L",   .permanentId = 70 },
+    { .boxId = BOXKNIFERIGHT,       .boxName = "KNIFE R",  .permanentId = 71 },
+    { .boxId = BOXPROPHANG,         .boxName = "P-HANG",         .permanentId = 72 },
+    { .boxId = BOXALTFLOOR,         .boxName = "FLOOR",         .permanentId = 73 },
+    { .boxId = BOXFIGROLL,          .boxName = "F ROLL",       .permanentId = 74 },
+    { .boxId = BOXFIGLOOP,          .boxName = "F LOOP",       .permanentId = 75 },
+    { .boxId = BOXFIGPOINTROLL,     .boxName = "F 4PT",   .permanentId = 76 },
+    { .boxId = BOXFIGSEQ,           .boxName = "F SEQ",        .permanentId = 77 },
+    { .boxId = BOXATTLOCK,          .boxName = "3DLOCK",           .permanentId = 78 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
 
@@ -283,6 +293,18 @@ void initActiveBoxIds(void)
         }
         if (sensors(SENSOR_ACC)) {
             ADD_ACTIVE_BOX(BOXANGLEHOLD);
+#ifdef USE_ORIENTATION_HOLD
+            ADD_ACTIVE_BOX(BOXINVERTED);
+            ADD_ACTIVE_BOX(BOXKNIFELEFT);
+            ADD_ACTIVE_BOX(BOXKNIFERIGHT);
+            ADD_ACTIVE_BOX(BOXPROPHANG);
+            ADD_ACTIVE_BOX(BOXALTFLOOR);
+            ADD_ACTIVE_BOX(BOXFIGROLL);
+            ADD_ACTIVE_BOX(BOXFIGLOOP);
+            ADD_ACTIVE_BOX(BOXFIGPOINTROLL);
+            ADD_ACTIVE_BOX(BOXFIGSEQ);
+            ADD_ACTIVE_BOX(BOXATTLOCK);
+#endif
         }
     }
 
@@ -449,6 +471,18 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION);
 #endif
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)),       BOXANGLEHOLD);
+#ifdef USE_ORIENTATION_HOLD
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXINVERTED)),        BOXINVERTED);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKNIFELEFT)),       BOXKNIFELEFT);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKNIFERIGHT)),      BOXKNIFERIGHT);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXPROPHANG)),        BOXPROPHANG);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXALTFLOOR)),        BOXALTFLOOR);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGROLL)),         BOXFIGROLL);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGLOOP)),         BOXFIGLOOP);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGPOINTROLL)),    BOXFIGPOINTROLL);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFIGSEQ)),          BOXFIGSEQ);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXATTLOCK)),         BOXATTLOCK);
+#endif
 
 #ifdef USE_SERIAL_GIMBAL
     if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 2e972d1b304..87e3130ceab 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -85,6 +85,16 @@ typedef enum {
     BOXGIMBALRLOCK   = 57,
     BOXGIMBALCENTER  = 58,
     BOXGIMBALHTRK    = 59,
+    BOXINVERTED      = 60,
+    BOXKNIFELEFT     = 61,
+    BOXKNIFERIGHT    = 62,
+    BOXPROPHANG      = 63,
+    BOXALTFLOOR      = 64,
+    BOXFIGROLL       = 65,
+    BOXFIGLOOP       = 66,
+    BOXFIGPOINTROLL  = 67,
+    BOXFIGSEQ        = 68,
+    BOXATTLOCK       = 69,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index faec9fdef15..428efcae6ac 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -108,6 +108,7 @@ typedef enum {
     ANGLEHOLD_MODE        = (1 << 17),
     NAV_FW_AUTOLAND       = (1 << 18),
     NAV_SEND_TO           = (1 << 19),
+    ORIENTATION_HOLD_MODE = (1 << 20),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 4e8affb0221..bd1c8e2c76f 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4502,3 +4502,135 @@ groups:
         field: noWayHomeAction
         table: geozone_rth_no_way_home
         type: uint8_t
+
+  - name: PG_ALTITUDE_FLOOR_CONFIG
+    type: altitudeFloorConfig_t
+    headers: ["flight/altitude_floor.h"]
+    condition: USE_ORIENTATION_HOLD
+    members:
+      - name: alt_floor_altitude
+        description: "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_value: 30
+        field: floorAltitude
+        min: 5
+        max: 500
+      - name: alt_floor_margin
+        description: "Margin above the altitude floor [m] to arm the floor after takeoff and to release the recovery"
+        default_value: 10
+        field: floorMargin
+        min: 2
+        max: 100
+      - name: alt_floor_climb_pitch
+        description: "Nose up pitch target [deg] flown during altitude floor recovery"
+        default_value: 15
+        field: floorClimbPitch
+        min: 5
+        max: 45
+
+  - name: PG_THRUST_VECTORING_CONFIG
+    type: thrustVectoringConfig_t
+    headers: ["flight/thrust_vectoring.h"]
+    condition: USE_THRUST_VECTORING
+    members:
+      - name: tvc_gain
+        description: "Overall thrust vectoring deflection gain [%] at full thrust, applied to the TVC servo mixer input sources"
+        default_value: 100
+        field: gain
+        min: 0
+        max: 200
+      - name: tvc_thrust_comp
+        description: "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_value: 100
+        field: thrustComp
+        min: 0
+        max: 100
+
+  - name: PG_ORIENTATION_HOLD_CONFIG
+    type: orientationHoldConfig_t
+    headers: ["flight/orientation_hold.h"]
+    condition: USE_ORIENTATION_HOLD
+    members:
+      - name: ohold_inverted_pitch_trim
+        description: "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_value: 0
+        field: invertedPitchTrim
+        min: -15
+        max: 15
+      - name: ohold_knife_left_pitch_trim
+        description: "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_value: 0
+        field: knifeLeftPitchTrim
+        min: -15
+        max: 15
+      - name: ohold_knife_right_pitch_trim
+        description: "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_value: 0
+        field: knifeRightPitchTrim
+        min: -15
+        max: 15
+
+  - name: PG_FIGURE_SEQUENCER_CONFIG
+    type: figureSequencerConfig_t
+    headers: ["flight/figure_sequencer.h"]
+    condition: USE_ORIENTATION_HOLD
+    members:
+      - name: fig_roll_rate
+        description: "Roll rate [deg/s] flown by the FIGURE ROLL and FIGURE 4PT ROLL modes"
+        default_value: 90
+        field: rollRate
+        min: 30
+        max: 360
+      - name: fig_loop_rate
+        description: "Pitch rate [deg/s] flown by the FIGURE LOOP mode"
+        default_value: 90
+        field: loopRate
+        min: 30
+        max: 360
+      - name: fig_point_dwell
+        description: "Dwell time [ms] on each point of the FIGURE 4PT ROLL"
+        default_value: 500
+        field: pointDwellMs
+        min: 100
+        max: 2000
+      - name: fig_assist_z_gain
+        description: "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_value: 20
+        field: assistZGain
+        min: 0
+        max: 100
+      - name: fig_assist_vz_gain
+        description: "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_value: 1
+        field: assistVzGain
+        min: 0
+        max: 20
+      - name: fig_assist_max
+        description: "Cap [deg] on the altitude assist nose-up offset"
+        default_value: 12
+        field: assistMax
+        min: 0
+        max: 30
+
+  - name: PG_HOVER_THROTTLE_CONFIG
+    type: hoverThrottleConfig_t
+    headers: ["flight/hover_throttle.h"]
+    condition: USE_ORIENTATION_HOLD
+    members:
+      - name: ohold_hover_thr_p
+        description: "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_value: 25
+        field: pGain
+        min: 0
+        max: 100
+      - name: ohold_hover_thr_i
+        description: "Hover throttle I gain [throttle us per m per second]"
+        default_value: 10
+        field: iGain
+        min: 0
+        max: 100
+      - name: ohold_hover_thr_d
+        description: "Hover throttle D gain [throttle us per m/s of climb rate]"
+        default_value: 30
+        field: dGain
+        min: 0
+        max: 100
diff --git a/src/main/flight/altitude_floor.c b/src/main/flight/altitude_floor.c
new file mode 100644
index 00000000000..6f8838cbd9d
--- /dev/null
+++ b/src/main/flight/altitude_floor.c
@@ -0,0 +1,107 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+
+#include <platform.h>
+
+#ifdef USE_ORIENTATION_HOLD
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
+#include "fc/settings.h"
+
+#include "flight/altitude_floor.h"
+
+#include "navigation/navigation.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(altitudeFloorConfig_t, altitudeFloorConfig, PG_ALTITUDE_FLOOR_CONFIG, 0);
+
+PG_RESET_TEMPLATE(altitudeFloorConfig_t, altitudeFloorConfig,
+    .floorAltitude = SETTING_ALT_FLOOR_ALTITUDE_DEFAULT,
+    .floorMargin = SETTING_ALT_FLOOR_MARGIN_DEFAULT,
+    .floorClimbPitch = SETTING_ALT_FLOOR_CLIMB_PITCH_DEFAULT,
+);
+
+// How far ahead the sink prediction looks. Must cover the roll-to-upright
+// time AND the Z estimator lag: under sustained sink the estimated altitude
+// trails the true altitude by roughly vz * estimator time constant (~2-3 s
+// with default baro weighting), so a short lookahead catches far too low.
+#define ALT_FLOOR_LOOKAHEAD_S 3.0f
+
+static bool floorArmed = false;      // climbed above floor + margin once
+static bool floorRecovery = false;
+
+void altitudeFloorUpdate(void)
+{
+    if (!IS_RC_MODE_ACTIVE(BOXALTFLOOR) || !ARMING_FLAG(ARMED) || !STATE(AIRPLANE)
+        || !navIsAltitudeEstimateTrusted()) {
+        floorArmed = false;
+        floorRecovery = false;
+        return;
+    }
+
+    const float z = getEstimatedActualPosition(Z);          // cm above home
+    const float vz = getEstimatedActualVelocity(Z);         // cm/s
+    const float floorCm = altitudeFloorConfig()->floorAltitude * 100.0f;
+    const float marginCm = altitudeFloorConfig()->floorMargin * 100.0f;
+
+    if (!floorArmed) {
+        // Arm only after climbing above floor + margin once, so switching
+        // the box on while on the ground (or arming below the floor) never
+        // grabs the aircraft during takeoff
+        floorArmed = z > (floorCm + marginCm);
+        return;
+    }
+
+    if (!floorRecovery) {
+        // Predictive engage: catch before the floor, not at it
+        if (vz < 0.0f && (z + vz * ALT_FLOOR_LOOKAHEAD_S) < floorCm) {
+            floorRecovery = true;
+        }
+    } else {
+        // Release when back above floor + margin and climbing
+        if (z > (floorCm + marginCm) && vz > 0.0f) {
+            floorRecovery = false;
+        }
+    }
+}
+
+bool altitudeFloorRecoveryActive(void)
+{
+    return floorRecovery;
+}
+
+float altitudeFloorRecoveryPitchDeg(void)
+{
+    return (float)altitudeFloorConfig()->floorClimbPitch;
+}
+
+#endif // USE_ORIENTATION_HOLD
diff --git a/src/main/flight/altitude_floor.h b/src/main/flight/altitude_floor.h
new file mode 100644
index 00000000000..1f35703dd18
--- /dev/null
+++ b/src/main/flight/altitude_floor.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "config/parameter_group.h"
+
+// Altitude floor ("training floor"): while the ALT FLOOR box is active and
+// the aircraft has climbed above floor + margin once, a predicted floor
+// breach engages an automatic recovery (shortest-path roll to upright plus
+// climb pitch via the orientation hold controller) until the aircraft is
+// back above the floor and climbing. Switch the box off to land.
+
+typedef struct altitudeFloorConfig_s {
+    uint16_t floorAltitude;     // m above home
+    uint16_t floorMargin;       // m above the floor to arm / release
+    uint8_t  floorClimbPitch;   // deg nose-up target during recovery
+} altitudeFloorConfig_t;
+
+PG_DECLARE(altitudeFloorConfig_t, altitudeFloorConfig);
+
+// Run once per RC processing cycle (before flight mode selection)
+void altitudeFloorUpdate(void);
+
+// True while the automatic recovery is flying the aircraft
+bool altitudeFloorRecoveryActive(void);
+
+// Recovery pitch target (deg, nose up)
+float altitudeFloorRecoveryPitchDeg(void);
diff --git a/src/main/flight/figure_sequencer.c b/src/main/flight/figure_sequencer.c
new file mode 100644
index 00000000000..83378e0e748
--- /dev/null
+++ b/src/main/flight/figure_sequencer.c
@@ -0,0 +1,388 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+
+#include <platform.h>
+
+#ifdef USE_ORIENTATION_HOLD
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/time.h"
+
+#include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
+#include "fc/settings.h"
+
+#include "flight/figure_sequencer.h"
+#include "flight/imu.h"
+
+#include "navigation/navigation.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(figureSequencerConfig_t, figureSequencerConfig, PG_FIGURE_SEQUENCER_CONFIG, 0);
+
+PG_RESET_TEMPLATE(figureSequencerConfig_t, figureSequencerConfig,
+    .rollRate = SETTING_FIG_ROLL_RATE_DEFAULT,
+    .loopRate = SETTING_FIG_LOOP_RATE_DEFAULT,
+    .pointDwellMs = SETTING_FIG_POINT_DWELL_DEFAULT,
+    .assistZGain = SETTING_FIG_ASSIST_Z_GAIN_DEFAULT,
+    .assistVzGain = SETTING_FIG_ASSIST_VZ_GAIN_DEFAULT,
+    .assistMax = SETTING_FIG_ASSIST_MAX_DEFAULT,
+);
+
+PG_REGISTER_ARRAY(figureSegment_t, MAX_FIGURE_SEQUENCE_SEGMENTS, figureSequence, PG_FIGURE_SEQUENCE, 0);
+
+typedef enum {
+    FIGURE_NONE = 0,
+    FIGURE_ROLL,
+    FIGURE_LOOP,
+    FIGURE_POINT_ROLL,
+    FIGURE_SEQUENCE,
+} figureType_e;
+
+typedef enum {
+    FIG_STATE_IDLE = 0,
+    FIG_STATE_RUNNING,
+    FIG_STATE_DONE,      // figure complete, holding level until box released
+} figureState_e;
+
+static figureType_e activeFigure = FIGURE_NONE;
+static figureState_e state = FIG_STATE_IDLE;
+static timeMs_t startTimeMs;
+static float startAltitudeCm;
+static float targetRollDeg;
+static float targetPitchDeg;
+
+// sequence (FIGURE SEQ) state
+static int seqIndex;
+static timeMs_t seqSegStartMs;
+static float seqBaseRoll;
+static float seqBasePitch;
+static float seqSegAltCm;      // assist reference, captured at segment entry
+static bool seqImpulseActive;
+static float seqImpulseRates[3];
+static bool seqTurnCoordination;
+static float seqTurnBankDeg;
+
+static figureType_e requestedFigure(void)
+{
+    if (IS_RC_MODE_ACTIVE(BOXFIGSEQ)) {
+        return FIGURE_SEQUENCE;
+    }
+    if (IS_RC_MODE_ACTIVE(BOXFIGROLL)) {
+        return FIGURE_ROLL;
+    }
+    if (IS_RC_MODE_ACTIVE(BOXFIGLOOP)) {
+        return FIGURE_LOOP;
+    }
+    if (IS_RC_MODE_ACTIVE(BOXFIGPOINTROLL)) {
+        return FIGURE_POINT_ROLL;
+    }
+    return FIGURE_NONE;
+}
+
+// Altitude assist: earth referenced nose-above-horizon offset from an
+// altitude/climb-rate PID, blended out as the nose approaches vertical
+// (there altitude is a thrust problem, not an attitude problem)
+static float altitudeAssistDeg(float nosePitchDeg, float refAltCm)
+{
+    if (!navIsAltitudeEstimateTrusted()) {
+        return 0.0f;
+    }
+
+    const float zErrM = (refAltCm - getEstimatedActualPosition(Z)) / 100.0f;
+    const float sinkMs = -getEstimatedActualVelocity(Z) / 100.0f;
+
+    float offset = (figureSequencerConfig()->assistZGain / 10.0f) * zErrM
+                 + figureSequencerConfig()->assistVzGain * sinkMs;
+    offset = constrainf(offset, -figureSequencerConfig()->assistMax, figureSequencerConfig()->assistMax);
+
+    // The offset must raise the NOSE ELEVATION. With an accumulated pitch
+    // parameter past +/-90 (e.g. base pitch 180 after a half loop) the raw
+    // pitch parameter acts inverted on the elevation: elevation = sin(pitch),
+    // d(elevation)/d(pitch) flips sign with cos(pitch). Blend out toward
+    // nose-vertical with |cos| (= cos of the true elevation).
+    const float cosPitch = cos_approx(DEGREES_TO_RADIANS(nosePitchDeg));
+    return offset * cosPitch;   // magnitude blends with |cos|, sign corrects the direction
+}
+
+void figureSequencerUpdate(void)
+{
+    const figureType_e req = requestedFigure();
+
+    if (req == FIGURE_NONE || !ARMING_FLAG(ARMED) || !STATE(AIRPLANE)) {
+        activeFigure = FIGURE_NONE;
+        state = FIG_STATE_IDLE;
+        return;
+    }
+
+    if (state == FIG_STATE_IDLE || req != activeFigure) {
+        activeFigure = req;
+        state = FIG_STATE_RUNNING;
+        startTimeMs = millis();
+        startAltitudeCm = getEstimatedActualPosition(Z);
+        seqIndex = 0;
+        seqSegStartMs = startTimeMs;
+        seqBaseRoll = 0.0f;
+        seqBasePitch = 0.0f;
+        seqSegAltCm = startAltitudeCm;
+    }
+
+    const float tS = (millis() - startTimeMs) * 0.001f;
+    float roll = 0.0f;
+    float pitch = 0.0f;
+    bool assist = false;
+    seqImpulseActive = false;   // recomputed below while an IMPULSE runs
+    seqTurnCoordination = false;
+
+    switch (activeFigure) {
+        case FIGURE_ROLL: {
+            const float theta = figureSequencerConfig()->rollRate * tS;
+            roll = MIN(theta, 360.0f);
+            assist = true;
+            if (theta >= 360.0f) {
+                state = FIG_STATE_DONE;
+                roll = 0.0f;    // 360 == 0, hold level
+            }
+            break;
+        }
+
+        case FIGURE_LOOP: {
+            const float theta = figureSequencerConfig()->loopRate * tS;
+            pitch = MIN(theta, 360.0f);
+            if (theta >= 360.0f) {
+                state = FIG_STATE_DONE;
+                pitch = 0.0f;
+                assist = true;  // level again: hold the entry altitude
+            }
+            break;
+        }
+
+        case FIGURE_SEQUENCE: {
+            // advance through the programmed segment chain
+            while (state == FIG_STATE_RUNNING) {
+                if (seqIndex >= MAX_FIGURE_SEQUENCE_SEGMENTS
+                    || figureSequence(seqIndex)->type == FIGSEG_END
+                    || figureSequence(seqIndex)->type >= FIGSEG_TYPE_COUNT) {
+                    state = FIG_STATE_DONE;
+                    break;
+                }
+
+                const figureSegment_t *seg = figureSequence(seqIndex);
+                const float tSeg = (millis() - seqSegStartMs) * 0.001f;
+                bool segDone = false;
+
+                switch (seg->type) {
+                    case FIGSEG_ROLL: {
+                        const float span = ABS((float)seg->p1);
+                        const float theta = MIN(figureSequencerConfig()->rollRate * tSeg, span);
+                        roll = seqBaseRoll + (seg->p1 < 0 ? -theta : theta);
+                        pitch = seqBasePitch;
+                        assist = seg->flags & FIGSEG_FLAG_ASSIST;
+                        if (theta >= span) {
+                            seqBaseRoll += seg->p1;
+                            segDone = true;
+                        }
+                        break;
+                    }
+
+                    case FIGSEG_PITCH: {
+                        const float span = ABS((float)seg->p1);
+                        const float theta = MIN(figureSequencerConfig()->loopRate * tSeg, span);
+                        roll = seqBaseRoll;
+                        pitch = seqBasePitch + (seg->p1 < 0 ? -theta : theta);
+                        assist = false;
+                        if (theta >= span) {
+                            seqBasePitch += seg->p1;
+                            segDone = true;
+                        }
+                        break;
+                    }
+
+                    case FIGSEG_HOLD:
+                        seqBaseRoll = seg->p1;
+                        seqBasePitch = seg->p2;
+                        roll = seqBaseRoll;
+                        pitch = seqBasePitch;
+                        assist = seg->flags & FIGSEG_FLAG_ASSIST;
+                        segDone = tSeg * 1000.0f >= seg->p3;
+                        break;
+
+                    case FIGSEG_WAIT_ALT: {
+                        // wings level, climb/descend to the target altitude
+                        // via the assist mechanism, gate until reached
+                        seqBaseRoll = 0.0f;
+                        seqBasePitch = 0.0f;
+                        roll = 0.0f;
+                        pitch = 0.0f;
+                        seqSegAltCm = seg->p1 * 100.0f;
+                        assist = true;
+                        const float tolCm = MAX(seg->p2, 1) * 100.0f;
+                        segDone = navIsAltitudeEstimateTrusted()
+                            && ABS(getEstimatedActualPosition(Z) - seqSegAltCm) < tolCm
+                            && ABS(getEstimatedActualVelocity(Z)) < 150.0f;
+                        break;
+                    }
+
+                    case FIGSEG_WAIT_TIME:
+                        roll = seqBaseRoll;
+                        pitch = seqBasePitch;
+                        assist = seg->flags & FIGSEG_FLAG_ASSIST;
+                        segDone = tSeg * 1000.0f >= seg->p3;
+                        break;
+
+                    case FIGSEG_IMPULSE:
+                        // open-loop rate impulse (snap/spin entry): full-rate
+                        // commands saturate the surfaces; the following
+                        // segment (or the DONE level hold) catches whatever
+                        // attitude results, shortest path
+                        seqImpulseActive = true;
+                        seqImpulseRates[FD_ROLL] = 0.0f;
+                        seqImpulseRates[FD_PITCH] = constrainf(seg->p1, -100, 100) * 0.01f;
+                        seqImpulseRates[FD_YAW] = constrainf(seg->p2, -100, 100) * 0.01f;
+                        roll = seqBaseRoll;
+                        pitch = seqBasePitch;
+                        segDone = tSeg * 1000.0f >= seg->p3;
+                        break;
+
+                    case FIGSEG_WAIT_POS: {
+                        // airspace containment: bank toward HOME until the
+                        // distance drops below the radius. The course loop
+                        // lives only in this segment; the attitude modes
+                        // stay heading-free
+                        seqBaseRoll = 0.0f;
+                        seqBasePitch = 0.0f;
+                        pitch = 0.0f;
+                        assist = true;
+                        if (STATE(GPS_FIX_HOME)) {
+                            const float maxBank = (seg->p2 > 0) ? seg->p2 : 30.0f;
+                            float courseErr = GPS_directionToHome - DECIDEGREES_TO_DEGREES((float)attitude.values.yaw);
+                            while (courseErr > 180.0f) { courseErr -= 360.0f; }
+                            while (courseErr < -180.0f) { courseErr += 360.0f; }
+                            roll = constrainf(0.8f * courseErr, -maxBank, maxBank);
+                            // banked turn: feed the coordinated turn rates
+                            // forward, otherwise the heading-free controller
+                            // regulates the (physical) turn yaw rate to zero
+                            seqTurnCoordination = true;
+                            seqTurnBankDeg = roll;
+                            segDone = GPS_distanceToHome < (uint32_t)MAX(seg->p1, 10);
+                        } else {
+                            roll = 0.0f;    // no home fix: hold level, gate stays
+                            segDone = false;
+                        }
+                        break;
+                    }
+
+                    default:
+                        segDone = true;
+                        break;
+                }
+
+                if (!segDone) {
+                    break;
+                }
+                seqIndex++;
+                seqSegStartMs = millis();
+                if (figureSequence(MIN(seqIndex, MAX_FIGURE_SEQUENCE_SEGMENTS - 1))->type != FIGSEG_WAIT_ALT) {
+                    seqSegAltCm = getEstimatedActualPosition(Z);   // assist reference for the next segment
+                }
+            }
+            if (state == FIG_STATE_DONE) {
+                roll = 0.0f;
+                pitch = 0.0f;
+                assist = true;
+            }
+            break;
+        }
+
+        case FIGURE_POINT_ROLL: {
+            // 4 points: rotate 90 deg at roll rate, dwell, repeat
+            const float rotS = 90.0f / figureSequencerConfig()->rollRate;
+            const float dwellS = figureSequencerConfig()->pointDwellMs / 1000.0f;
+            const float segS = rotS + dwellS;
+            const int seg = (int)(tS / segS);
+            if (seg >= 4) {
+                state = FIG_STATE_DONE;
+                roll = 0.0f;
+            } else {
+                const float tInSeg = tS - seg * segS;
+                roll = seg * 90.0f + MIN(tInSeg / rotS, 1.0f) * 90.0f;
+            }
+            assist = true;
+            break;
+        }
+
+        default:
+            break;
+    }
+
+    if (state == FIG_STATE_DONE) {
+        roll = (activeFigure == FIGURE_LOOP) ? 0.0f : roll;
+        assist = true;
+    }
+
+    targetRollDeg = roll;
+    targetPitchDeg = pitch + (assist
+        ? altitudeAssistDeg(pitch, activeFigure == FIGURE_SEQUENCE ? seqSegAltCm : startAltitudeCm)
+        : 0.0f);
+}
+
+bool figureSequencerRequested(void)
+{
+    return activeFigure != FIGURE_NONE && state != FIG_STATE_IDLE;
+}
+
+bool figureSequencerGetTurnBank(float *bankDeg)
+{
+    if (!seqTurnCoordination) {
+        return false;
+    }
+    *bankDeg = seqTurnBankDeg;
+    return true;
+}
+
+bool figureSequencerGetRateCommand(float ratesNorm[3])
+{
+    if (!seqImpulseActive) {
+        return false;
+    }
+    for (int i = 0; i < 3; i++) {
+        ratesNorm[i] = seqImpulseRates[i];
+    }
+    return true;
+}
+
+void figureSequencerGetTarget(float *rollDeg, float *pitchDeg)
+{
+    *rollDeg = targetRollDeg;
+    *pitchDeg = targetPitchDeg;
+}
+
+#endif // USE_ORIENTATION_HOLD
diff --git a/src/main/flight/figure_sequencer.h b/src/main/flight/figure_sequencer.h
new file mode 100644
index 00000000000..02c81657aa0
--- /dev/null
+++ b/src/main/flight/figure_sequencer.h
@@ -0,0 +1,106 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "config/parameter_group.h"
+
+// Figure sequencer: flies aerobatic figures as time parameterized
+// orientation-hold targets (roll/pitch trajectories). Because the
+// orientation hold controller is heading free, a figure needs no attitude
+// capture: a roll always rotates about the current heading, a loop flies in
+// the current heading plane. Figures start when their box goes active
+// (from roughly level flight), hold level when complete, and re-arm when
+// the box is released.
+//
+// Altitude assist: a PID on altitude/climb rate adds an earth referenced
+// "nose above horizon" offset to the figure target. The controller
+// distributes it to elevator and rudder as the roll phase demands (the
+// classic slow-roll coordination), blended out as the nose approaches
+// vertical where altitude belongs to the thrust axis.
+
+typedef struct figureSequencerConfig_s {
+    uint16_t rollRate;        // deg/s target roll rate for roll figures
+    uint16_t loopRate;        // deg/s target pitch rate for the loop
+    uint16_t pointDwellMs;    // ms dwell on each point of the 4 point roll
+    uint8_t  assistZGain;     // deg of nose-up per 10 m of altitude error
+    uint8_t  assistVzGain;    // deg of nose-up per m/s of sink
+    uint8_t  assistMax;       // deg cap on the altitude assist offset
+} figureSequencerConfig_t;
+
+PG_DECLARE(figureSequencerConfig_t, figureSequencerConfig);
+
+// Programmable figure sequence (FIGURE SEQ box): a chain of segments flown
+// in order. Rotations are cumulative on the running attitude baseline, so
+// e.g. Immelmann = PITCH +180 then ROLL +180. Wait segments gate the chain
+// on preconditions (altitude now; position is reserved, it needs heading
+// control / nav coupling).
+#define MAX_FIGURE_SEQUENCE_SEGMENTS 16
+
+typedef enum {
+    FIGSEG_END = 0,          // terminator / unused slot
+    FIGSEG_ROLL = 1,         // p1: signed deg, cumulative, at fig_roll_rate
+    FIGSEG_PITCH = 2,        // p1: signed deg, cumulative, at fig_loop_rate
+    FIGSEG_HOLD = 3,         // p1: roll deg, p2: pitch deg (absolute), p3: ms
+    FIGSEG_WAIT_ALT = 4,     // p1: target altitude m above home, p2: tolerance m
+    FIGSEG_WAIT_TIME = 5,    // p3: ms, holds the current baseline attitude
+    FIGSEG_IMPULSE = 6,      // open-loop rate impulse (snap/spin entry):
+                             // p1: pitch %, p2: yaw %, p3: ms; the next
+                             // segment catches the attitude afterwards
+    FIGSEG_WAIT_POS = 7,     // airspace containment: bank toward HOME until
+                             // distance < p1 m; p2: max bank deg (0 = 30)
+    FIGSEG_TYPE_COUNT
+} figureSegmentType_e;
+
+#define FIGSEG_FLAG_ASSIST (1 << 0)   // altitude assist during this segment
+
+typedef struct figureSegment_s {
+    uint8_t type;
+    int16_t p1;
+    int16_t p2;
+    int16_t p3;
+    uint8_t flags;
+} figureSegment_t;
+
+PG_DECLARE_ARRAY(figureSegment_t, MAX_FIGURE_SEQUENCE_SEGMENTS, figureSequence);
+
+// Run once per RC processing cycle (before flight mode selection)
+void figureSequencerUpdate(void);
+
+// True while a figure box is active (sequencer wants ORIENTATION_HOLD_MODE)
+bool figureSequencerRequested(void);
+
+// Current figure target, valid while requested
+void figureSequencerGetTarget(float *rollDeg, float *pitchDeg);
+
+// True while an open-loop IMPULSE segment runs; returns the commanded body
+// rates normalized to -1..1 of the profile's max rates (full deflection)
+bool figureSequencerGetRateCommand(float ratesNorm[3]);
+
+// True while a WAIT_POS segment banks toward home; returns the commanded
+// bank (deg) for coordinated-turn rate feedforward
+bool figureSequencerGetTurnBank(float *bankDeg);
diff --git a/src/main/flight/hover_throttle.c b/src/main/flight/hover_throttle.c
new file mode 100644
index 00000000000..cd0fa4f4695
--- /dev/null
+++ b/src/main/flight/hover_throttle.c
@@ -0,0 +1,149 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <math.h>
+
+#include <platform.h>
+
+#ifdef USE_ORIENTATION_HOLD
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/quaternion.h"
+#include "common/vector.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/time.h"
+
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+#include "fc/settings.h"
+
+#include "flight/hover_throttle.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/orientation_hold.h"
+
+#include "navigation/navigation.h"
+
+#include "rx/rx.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig, PG_HOVER_THROTTLE_CONFIG, 0);
+
+PG_RESET_TEMPLATE(hoverThrottleConfig_t, hoverThrottleConfig,
+    .pGain = SETTING_OHOLD_HOVER_THR_P_DEFAULT,
+    .iGain = SETTING_OHOLD_HOVER_THR_I_DEFAULT,
+    .dGain = SETTING_OHOLD_HOVER_THR_D_DEFAULT,
+);
+
+// Engage only when the nose is this close to the zenith; once engaged,
+// stay active down to the release threshold. Without the hysteresis the
+// attitude wobble around the hang flaps the controller and every
+// re-engage captures a NEW altitude target at the current height -- a
+// ratcheting drift.
+#define HOVER_ENGAGE_NOSE_ELEVATION_DEG  60.0f
+#define HOVER_RELEASE_NOSE_ELEVATION_DEG 45.0f
+
+// The target latches only once the vertical motion has settled: engaging
+// mid pull-up (the normal way to enter a hang) must not freeze the target
+// at some fly-through altitude
+#define HOVER_LATCH_CLIMB_CMS 200.0f
+
+static bool hoverActive = false;
+static bool hoverLatched = false;
+static float targetAltCm;
+static float iTermUs;
+static timeUs_t lastUpdateUs;
+
+static float noseElevationDeg(void)
+{
+    fpVector3_t nose = { .v = { 1.0f, 0.0f, 0.0f } };
+    quaternionRotateVectorInv(&nose, &nose, &orientation);   // body -> earth
+    return RADIANS_TO_DEGREES(asin_approx(constrainf(-nose.z, -1.0f, 1.0f)));
+}
+
+int16_t hoverThrottleApply(int16_t pilotThrottle)
+{
+    const float elevDeg = noseElevationDeg();
+    const float elevGate = hoverActive ? HOVER_RELEASE_NOSE_ELEVATION_DEG
+                                       : HOVER_ENGAGE_NOSE_ELEVATION_DEG;
+
+    if (!ARMING_FLAG(ARMED)
+        || !orientationHoldIsPropHang()
+        || !navIsAltitudeEstimateTrusted()
+        || elevDeg < elevGate) {
+        hoverActive = false;
+        return pilotThrottle;
+    }
+
+    const float z = getEstimatedActualPosition(Z);
+
+    // pilot throttle outside the mid deadband: direct control, target follows
+    if (ABS(pilotThrottle - PWM_RANGE_MIDDLE) > rcControlsConfig()->mid_throttle_deadband) {
+        hoverActive = false;
+        return pilotThrottle;
+    }
+
+    if (!hoverActive) {
+        hoverActive = true;
+        hoverLatched = false;
+        targetAltCm = z;
+        // seed the I-term with the last pilot throttle: learns the model's
+        // hover throttle online instead of requiring a setting
+        iTermUs = pilotThrottle;
+        lastUpdateUs = micros();
+    }
+
+    const timeUs_t nowUs = micros();
+    const float dT = constrainf((nowUs - lastUpdateUs) * 1e-6f, 0.0f, 0.1f);
+    lastUpdateUs = nowUs;
+
+    const float climbCms = getEstimatedActualVelocity(Z);
+    if (!hoverLatched) {
+        targetAltCm = z;                       // follow until motion settles
+        if (fabsf(climbCms) < HOVER_LATCH_CLIMB_CMS) {
+            hoverLatched = true;
+        }
+    }
+
+    const float zErrM = (targetAltCm - z) / 100.0f;
+    const float climbMs = climbCms / 100.0f;
+
+    iTermUs = constrainf(iTermUs + hoverThrottleConfig()->iGain * zErrM * dT,
+                         getThrottleIdleValue(), getMaxThrottle());
+
+    // thrust supports the weight with its vertical component only:
+    // compensate the tilt away from the zenith (capped, the elevation
+    // gate keeps this bounded anyway)
+    const float vertical = constrainf(sin_approx(DEGREES_TO_RADIANS(elevDeg)), 0.5f, 1.0f);
+    const float correction = (hoverThrottleConfig()->pGain * zErrM
+                              - hoverThrottleConfig()->dGain * climbMs) / vertical;
+
+    return constrain(lrintf(iTermUs + correction), getThrottleIdleValue(), getMaxThrottle());
+}
+
+#endif // USE_ORIENTATION_HOLD
diff --git a/src/main/flight/hover_throttle.h b/src/main/flight/hover_throttle.h
new file mode 100644
index 00000000000..fea92fb383d
--- /dev/null
+++ b/src/main/flight/hover_throttle.h
@@ -0,0 +1,48 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include "config/parameter_group.h"
+
+// Hover throttle: while PROP HANG is held (nose near vertical) the thrust
+// carries the weight and owns the altitude axis. A dedicated throttle PID
+// holds the altitude captured at engage; its I-term is seeded from the
+// pilot's throttle (learning the hover throttle online) and the output is
+// tilt compensated. Moving the throttle stick out of the mid deadband
+// hands control back to the pilot and re-captures the target.
+
+typedef struct hoverThrottleConfig_s {
+    uint8_t pGain;    // throttle us per m of altitude error
+    uint8_t iGain;    // throttle us per m per second
+    uint8_t dGain;    // throttle us per m/s of climb rate
+} hoverThrottleConfig_t;
+
+PG_DECLARE(hoverThrottleConfig_t, hoverThrottleConfig);
+
+// Called from the mixer throttle path; returns the pilot throttle when the
+// hover throttle is not active, the controller output otherwise.
+int16_t hoverThrottleApply(int16_t pilotThrottle);
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index a80992b772d..4a9a7558dbf 100644
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -46,6 +46,7 @@
 #include "fc/settings.h"
 
 #include "flight/failsafe.h"
+#include "flight/hover_throttle.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
@@ -587,7 +588,12 @@ void FAST_CODE mixTable(void)
         }
 #endif
     } else {
+#ifdef USE_ORIENTATION_HOLD
+        // hover throttle owns the altitude axis while PROP HANG is held
+        mixerThrottleCommand = hoverThrottleApply(rcCommand[THROTTLE]);
+#else
         mixerThrottleCommand = rcCommand[THROTTLE];
+#endif
         throttleRangeMin = throttleIdleValue;
         throttleRangeMax = getMaxThrottle();
 
diff --git a/src/main/flight/orientation_hold.c b/src/main/flight/orientation_hold.c
new file mode 100644
index 00000000000..7c87770eb71
--- /dev/null
+++ b/src/main/flight/orientation_hold.c
@@ -0,0 +1,265 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <math.h>
+#include <stdbool.h>
+
+#include <platform.h>
+
+#ifdef USE_ORIENTATION_HOLD
+
+#include "common/maths.h"
+#include "common/quaternion.h"
+#include "common/utils.h"
+#include "common/vector.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
+#include "fc/settings.h"
+
+#include "flight/altitude_floor.h"
+#include "flight/figure_sequencer.h"
+#include "flight/imu.h"
+#include "flight/orientation_hold.h"
+#include "flight/pid.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig, PG_ORIENTATION_HOLD_CONFIG, 0);
+
+PG_RESET_TEMPLATE(orientationHoldConfig_t, orientationHoldConfig,
+    .invertedPitchTrim = SETTING_OHOLD_INVERTED_PITCH_TRIM_DEFAULT,
+    .knifeLeftPitchTrim = SETTING_OHOLD_KNIFE_LEFT_PITCH_TRIM_DEFAULT,
+    .knifeRightPitchTrim = SETTING_OHOLD_KNIFE_RIGHT_PITCH_TRIM_DEFAULT,
+);
+
+typedef struct {
+    boxId_e box;
+    float rollDeg;
+    float pitchDeg;
+} orientationHoldPreset_t;
+
+// First matching box wins. Targets are tilt only (yaw = 0); heading is
+// always left free by the twist removal in the error computation.
+static const orientationHoldPreset_t orientationHoldPresets[] = {
+    { BOXINVERTED,      180.0f,  0.0f },
+    { BOXKNIFELEFT,     -90.0f,  0.0f },
+    { BOXKNIFERIGHT,     90.0f,  0.0f },
+    { BOXPROPHANG,        0.0f, 90.0f },
+};
+
+static const orientationHoldPreset_t * orientationHoldActivePreset(void)
+{
+    for (unsigned i = 0; i < ARRAYLEN(orientationHoldPresets); i++) {
+        if (IS_RC_MODE_ACTIVE(orientationHoldPresets[i].box)) {
+            return &orientationHoldPresets[i];
+        }
+    }
+    return NULL;
+}
+
+bool orientationHoldIsRequested(void)
+{
+    return figureSequencerRequested() || orientationHoldActivePreset() != NULL
+        || IS_RC_MODE_ACTIVE(BOXATTLOCK);
+}
+
+static bool orientationHoldSticksDeflected(void)
+{
+    return ABS(rcCommand[ROLL]) > rcControlsConfig()->deadband
+        || ABS(rcCommand[PITCH]) > rcControlsConfig()->deadband
+        || ABS(rcCommand[YAW]) > rcControlsConfig()->yaw_deadband;
+}
+
+// Same Euler to quaternion convention as imuComputeQuaternionFromRPY (yaw = 0)
+void orientationHoldTargetFromRP(fpQuaternion_t *qTarget, float rollDeg, float pitchDeg)
+{
+    const float cosRoll = cos_approx(DEGREES_TO_RADIANS(rollDeg) * 0.5f);
+    const float sinRoll = sin_approx(DEGREES_TO_RADIANS(rollDeg) * 0.5f);
+    const float cosPitch = cos_approx(DEGREES_TO_RADIANS(pitchDeg) * 0.5f);
+    const float sinPitch = sin_approx(DEGREES_TO_RADIANS(pitchDeg) * 0.5f);
+
+    qTarget->q0 = cosRoll * cosPitch;
+    qTarget->q1 = sinRoll * cosPitch;
+    qTarget->q2 = cosRoll * sinPitch;
+    qTarget->q3 = -sinRoll * sinPitch;
+}
+
+// Earth vertical (up) expressed in the body frame, normalized. Works for a
+// slightly denormalized quaternion as well.
+static void earthUpInBodyFrame(fpVector3_t *up, const fpQuaternion_t *q)
+{
+    fpVector3_t v = { .v = { 0.0f, 0.0f, 1.0f } };
+    quaternionRotateVector(&v, &v, q);
+    const float norm = fast_fsqrtf(sq(v.x) + sq(v.y) + sq(v.z));
+    up->x = v.x / norm;
+    up->y = v.y / norm;
+    up->z = v.z / norm;
+}
+
+void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget)
+{
+    // Reduced attitude control: regulate the direction of the earth vertical
+    // in the body frame instead of the full rotation. The rotation about the
+    // vertical axis (heading in normal/inverted flight, body roll at prop
+    // hang) is free by construction. Unlike a swing-twist decomposition
+    // about earth Z this has no degenerate region near inverted, where
+    // w^2 + z^2 vanishes for every heading and the extracted twist direction
+    // is noise driven.
+    fpVector3_t upEst, upTarget;
+    earthUpInBodyFrame(&upEst, qEst);
+    earthUpInBodyFrame(&upTarget, qTarget);
+
+    // Shortest rotation taking upEst to upTarget; operand order gives the
+    // pidLevel() sign convention (error = target - attitude), pinned by the
+    // host convention tests
+    fpVector3_t cross = { .v = {
+        upTarget.y * upEst.z - upTarget.z * upEst.y,
+        upTarget.z * upEst.x - upTarget.x * upEst.z,
+        upTarget.x * upEst.y - upTarget.y * upEst.x,
+    }};
+    const float crossNorm = fast_fsqrtf(sq(cross.x) + sq(cross.y) + sq(cross.z));
+    const float dot = upEst.x * upTarget.x + upEst.y * upTarget.y + upEst.z * upTarget.z;
+    const float angle = atan2_approx(crossNorm, dot);
+
+    fpVector3_t axis;
+    if (crossNorm > 1e-6f) {
+        axis.x = cross.x / crossNorm;
+        axis.y = cross.y / crossNorm;
+        axis.z = cross.z / crossNorm;
+    } else if (dot < 0.0f) {
+        // Exactly 180 deg of tilt error: rotation axis is ambiguous, pick a
+        // deterministic body axis orthogonal to the target up direction
+        // (the one least aligned with it)
+        fpVector3_t seed = { .v = { 0.0f, 0.0f, 0.0f } };
+        if (fabsf(upTarget.x) <= fabsf(upTarget.y) && fabsf(upTarget.x) <= fabsf(upTarget.z)) {
+            seed.x = 1.0f;
+        } else if (fabsf(upTarget.y) <= fabsf(upTarget.z)) {
+            seed.y = 1.0f;
+        } else {
+            seed.z = 1.0f;
+        }
+        axis.x = upTarget.y * seed.z - upTarget.z * seed.y;
+        axis.y = upTarget.z * seed.x - upTarget.x * seed.z;
+        axis.z = upTarget.x * seed.y - upTarget.y * seed.x;
+        const float norm = fast_fsqrtf(sq(axis.x) + sq(axis.y) + sq(axis.z));
+        axis.x /= norm;
+        axis.y /= norm;
+        axis.z /= norm;
+    } else {
+        errDeg->x = errDeg->y = errDeg->z = 0.0f;
+        return;
+    }
+
+    // Sign matches pidLevel(): error = target - attitude
+    errDeg->x = RADIANS_TO_DEGREES(axis.x * angle);
+    errDeg->y = RADIANS_TO_DEGREES(axis.y * angle);
+    errDeg->z = RADIANS_TO_DEGREES(axis.z * angle);
+}
+
+// Rate-loop I-term reset on target-source switches (e.g. prop hang ->
+// knife edge): the accumulated I trims the OLD attitude's holding load
+// (propwash vs knife rudder load) and would discharge as a disturbance
+// into the new attitude. Within a figure (continuous trajectory) the
+// source stays the same and the I-term is kept.
+#define OHOLD_SOURCE_NONE   (-1)
+#define OHOLD_SOURCE_FLOOR  (-2)
+#define OHOLD_SOURCE_FIGURE (-3)
+#define OHOLD_SOURCE_LOCK   (-4)
+
+static int activeTargetSource = OHOLD_SOURCE_NONE;
+
+static void orientationHoldCheckSourceSwitch(int source)
+{
+    if (source != activeTargetSource) {
+        if (activeTargetSource != OHOLD_SOURCE_NONE || source != OHOLD_SOURCE_NONE) {
+            pidResetErrorAccumulators();
+        }
+        activeTargetSource = source;
+    }
+}
+
+bool orientationHoldIsPropHang(void)
+{
+    return activeTargetSource == BOXPROPHANG;
+}
+
+void orientationHoldResetSourceTracking(void)
+{
+    // mode left: also reset, the attitude's holding-load trim in the I-term
+    // would discharge into the pilot's manual/acro flying otherwise
+    if (activeTargetSource != OHOLD_SOURCE_NONE) {
+        pidResetErrorAccumulators();
+        activeTargetSource = OHOLD_SOURCE_NONE;
+    }
+}
+
+bool orientationHoldComputeError(fpVector3_t *errDeg)
+{
+    fpQuaternion_t qTarget;
+
+    // Altitude floor recovery overrides any selected preset: upright + climb
+    if (altitudeFloorRecoveryActive()) {
+        orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FLOOR);
+        orientationHoldTargetFromRP(&qTarget, 0.0f, altitudeFloorRecoveryPitchDeg());
+    } else if (figureSequencerRequested()) {
+        float figRoll, figPitch;
+        orientationHoldCheckSourceSwitch(OHOLD_SOURCE_FIGURE);
+        figureSequencerGetTarget(&figRoll, &figPitch);
+        orientationHoldTargetFromRP(&qTarget, figRoll, figPitch);
+    } else if (orientationHoldActivePreset() == NULL && IS_RC_MODE_ACTIVE(BOXATTLOCK)) {
+        // 3D LOCK: sticks centered = hold the attitude captured at release;
+        // sticks deflected = pure rate flying, the lock target follows the
+        // aircraft and freezes on the NEW attitude when the sticks center
+        static fpQuaternion_t lockTarget;
+        if (activeTargetSource != OHOLD_SOURCE_LOCK || orientationHoldSticksDeflected()) {
+            lockTarget = orientation;
+        }
+        orientationHoldCheckSourceSwitch(OHOLD_SOURCE_LOCK);
+        qTarget = lockTarget;
+    } else {
+        const orientationHoldPreset_t *preset = orientationHoldActivePreset();
+        if (!preset) {
+            return false;
+        }
+        orientationHoldCheckSourceSwitch(preset->box);
+        // Per attitude pitch trim, as Euler pitch of the target: positive is
+        // always "nose above the horizon" regardless of the attitude's roll
+        float pitchTrim = 0.0f;
+        if (preset->box == BOXINVERTED) {
+            pitchTrim = orientationHoldConfig()->invertedPitchTrim;
+        } else if (preset->box == BOXKNIFELEFT) {
+            pitchTrim = orientationHoldConfig()->knifeLeftPitchTrim;
+        } else if (preset->box == BOXKNIFERIGHT) {
+            pitchTrim = orientationHoldConfig()->knifeRightPitchTrim;
+        }
+        orientationHoldTargetFromRP(&qTarget, preset->rollDeg, preset->pitchDeg + pitchTrim);
+    }
+
+    orientationHoldComputeAttitudeError(errDeg, &orientation, &qTarget);
+    return true;
+}
+
+#endif // USE_ORIENTATION_HOLD
diff --git a/src/main/flight/orientation_hold.h b/src/main/flight/orientation_hold.h
new file mode 100644
index 00000000000..642ea46c446
--- /dev/null
+++ b/src/main/flight/orientation_hold.h
@@ -0,0 +1,78 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "common/quaternion.h"
+#include "common/vector.h"
+
+#include "config/parameter_group.h"
+
+// Orientation hold: quaternion based attitude controller that can stabilise
+// arbitrary target attitudes (inverted, knife edge, prop hang) on fixed wing.
+// Unlike the Euler based ANGLE controller it has no singularity at
+// pitch = +/-90 deg. Heading (rotation about the earth vertical axis) is
+// always left free, matching ANGLE mode behaviour in normal flight.
+
+// Per attitude pitch trim on the hold target, applied as the Euler pitch of
+// the target (before the attitude's roll): positive = nose above the
+// horizon in every attitude. Inverted flight needs it to hold altitude
+// (down-elevator bias), knife edge to carry the fuselage lift.
+typedef struct orientationHoldConfig_s {
+    int8_t invertedPitchTrim;      // deg, nose above horizon in inverted flight
+    int8_t knifeLeftPitchTrim;     // deg, nose above horizon in left knife edge
+    int8_t knifeRightPitchTrim;    // deg, nose above horizon in right knife edge
+                                   // (separate per side: prop effects break the symmetry)
+} orientationHoldConfig_t;
+
+PG_DECLARE(orientationHoldConfig_t, orientationHoldConfig);
+
+// Compute the body frame attitude error (deg, per body axis) between qEst
+// and the tilt part of qTarget. The rotation of qEst about the earth
+// vertical axis (heading / twist) is removed before the error is formed, so
+// the returned error never asks for a heading change.
+void orientationHoldComputeAttitudeError(fpVector3_t *errDeg, const fpQuaternion_t *qEst, const fpQuaternion_t *qTarget);
+
+// Build a target quaternion from roll/pitch (deg, yaw = 0) using the same
+// Euler convention as the attitude estimator.
+void orientationHoldTargetFromRP(fpQuaternion_t *qTarget, float rollDeg, float pitchDeg);
+
+// True when any orientation hold box (INVERTED / KNIFE EDGE / PROP HANG) is
+// selected on the transmitter.
+bool orientationHoldIsRequested(void);
+
+// Body frame attitude error (deg) for the currently selected target.
+// Returns false when no orientation hold box is active.
+bool orientationHoldComputeError(fpVector3_t *errDeg);
+
+// Call while ORIENTATION_HOLD_MODE is inactive: resets the target-source
+// tracking (and the rate-loop I accumulators once on the exit edge)
+void orientationHoldResetSourceTracking(void);
+
+// True while the PROP HANG preset is the active hold target (used by the
+// hover throttle to own the altitude axis)
+bool orientationHoldIsPropHang(void);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 9beda7ae225..0bd0307d9d9 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -43,7 +43,9 @@
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
+#include "flight/figure_sequencer.h"
 #include "flight/mixer_profile.h"
+#include "flight/orientation_hold.h"
 #include "flight/rpm_filter.h"
 #include "flight/kalman.h"
 #include "flight/smith_predictor.h"
@@ -725,6 +727,47 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam
     }
 }
 
+#ifdef USE_ORIENTATION_HOLD
+// Quaternion based attitude hold for arbitrary target attitudes (inverted,
+// knife edge, prop hang). Works on all three body axes and stays defined at
+// pitch = +/-90 deg where the Euler based pidLevel() is singular. Sticks
+// remain live as rate commands on top of the stabilisation.
+static void NOINLINE pidOrientationHold(pidState_t *pidStates, float dT)
+{
+    fpVector3_t errDeg;
+
+    // open-loop rate impulse (figure sequencer snap/spin entry): command
+    // the profile's full rates directly, saturating the surfaces
+    float impulseNorm[3];
+    if (figureSequencerGetRateCommand(impulseNorm)) {
+        for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
+            pidStates[axis].rateTarget = constrainf(
+                impulseNorm[axis] * currentControlProfile->stabilized.rates[axis] * 10.0f,
+                -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
+        }
+        return;
+    }
+
+    if (!orientationHoldComputeError(&errDeg)) {
+        return;
+    }
+
+    for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
+        // Same gain and rate limit handling as pidLevel()
+        float rateTarget = constrainf(errDeg.v[axis] * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER),
+                                      -currentControlProfile->stabilized.rates[axis] * 10.0f,
+                                       currentControlProfile->stabilized.rates[axis] * 10.0f);
+
+        if (pidBank()->pid[PID_LEVEL].I) {
+            // I8[PIDLEVEL] is used as a PT1 cutoff frequency (Hz), same as pidLevel()
+            rateTarget = pt1FilterApply4(&pidStates[axis].angleFilterState, rateTarget, pidBank()->pid[PID_LEVEL].I, dT);
+        }
+
+        pidStates[axis].rateTarget = constrainf(pidStates[axis].rateTarget + rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
+    }
+}
+#endif
+
 /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
 static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 {
@@ -1277,6 +1320,14 @@ void FAST_CODE pidController(float dT)
     const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
     angleHoldIsLevel = false;
 
+#ifdef USE_ORIENTATION_HOLD
+    if (FLIGHT_MODE(ORIENTATION_HOLD_MODE)) {
+        // Quaternion attitude hold replaces the Euler level controllers on all three axes
+        pidOrientationHold(pidState, dT);
+        restartAngleHoldMode = true;
+        canUseFpvCameraMix = false;     // not compatible with FPVANGLEMIX
+    } else
+#endif
     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)) {
             // If axis angle override, get the correct angle from Logic Conditions
@@ -1306,6 +1357,17 @@ void FAST_CODE pidController(float dT)
         pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
     }
+#ifdef USE_ORIENTATION_HOLD
+    else if (FLIGHT_MODE(ORIENTATION_HOLD_MODE)) {
+        // WAIT_POS banks toward home: feed the coordinated turn rates
+        // forward, otherwise the heading-free hold regulates the physical
+        // turn yaw rate back to zero and the aircraft never turns
+        float bankDeg;
+        if (figureSequencerGetTurnBank(&bankDeg)) {
+            pidTurnAssistant(pidState, DEGREES_TO_RADIANS(bankDeg), 0.0f);
+        }
+    }
+#endif
 
     // Apply FPV camera mix
     if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) {
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index e72dd66ea7d..046c454b144 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -53,6 +53,7 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/servos.h"
+#include "flight/thrust_vectoring.h"
 
 #include "io/gps.h"
 
@@ -353,6 +354,18 @@ void servoMixer(float dT)
 
     input[INPUT_STABILIZED_THROTTLE] = mixerThrottleCommand - 1000 - 500;  // Since it derives from rcCommand or mincommand and must be [-500:+500]
 
+#ifdef USE_THRUST_VECTORING
+    {
+        // Same stabilized commands as the surfaces, but with inverse thrust
+        // compensation so vectoring vane / tilt motor authority stays
+        // roughly constant across the throttle range
+        const float tvcGain = thrustVectoringGain((mixerThrottleCommand - 1000) / 1000.0f);
+        input[INPUT_TVC_ROLL] = constrain(lrintf(input[INPUT_STABILIZED_ROLL] * tvcGain), -1000, 1000);
+        input[INPUT_TVC_PITCH] = constrain(lrintf(input[INPUT_STABILIZED_PITCH] * tvcGain), -1000, 1000);
+        input[INPUT_TVC_YAW] = constrain(lrintf(input[INPUT_STABILIZED_YAW] * tvcGain), -1000, 1000);
+    }
+#endif
+
     input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing * 500; //fixed value
     input[INPUT_MIXER_SWITCH_HELPER] = 0; // no input, used to apply speed limit filter from previous servo rules
 
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index 3f8ebf1b4e8..6b2872ceb08 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -85,6 +85,9 @@ typedef enum {
     INPUT_RC_CH33                   = 58,
     INPUT_RC_CH34                   = 59,
     INPUT_MIXER_SWITCH_HELPER       = 60,
+    INPUT_TVC_ROLL                  = 61,
+    INPUT_TVC_PITCH                 = 62,
+    INPUT_TVC_YAW                   = 63,
     INPUT_SOURCE_COUNT
 } inputSource_e;
 
diff --git a/src/main/flight/thrust_vectoring.c b/src/main/flight/thrust_vectoring.c
new file mode 100644
index 00000000000..148ca7b8913
--- /dev/null
+++ b/src/main/flight/thrust_vectoring.c
@@ -0,0 +1,57 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <platform.h>
+
+#ifdef USE_THRUST_VECTORING
+
+#include "common/maths.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "fc/settings.h"
+
+#include "flight/thrust_vectoring.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(thrustVectoringConfig_t, thrustVectoringConfig, PG_THRUST_VECTORING_CONFIG, 0);
+
+PG_RESET_TEMPLATE(thrustVectoringConfig_t, thrustVectoringConfig,
+    .gain = SETTING_TVC_GAIN_DEFAULT,
+    .thrustComp = SETTING_TVC_THRUST_COMP_DEFAULT,
+);
+
+// Below this thrust fraction the compensation stops growing: vane authority
+// is gone anyway and the servos should not flail against the stops
+#define TVC_THRUST_COMP_FLOOR 0.25f
+
+float thrustVectoringGain(float thrustFraction)
+{
+    const float t = constrainf(thrustFraction, TVC_THRUST_COMP_FLOOR, 1.0f);
+    const float fullComp = 1.0f / t;    // 1 .. 1/floor
+    const float comp = 1.0f + (fullComp - 1.0f) * (thrustVectoringConfig()->thrustComp / 100.0f);
+    return (thrustVectoringConfig()->gain / 100.0f) * comp;
+}
+
+#endif // USE_THRUST_VECTORING
diff --git a/src/main/flight/thrust_vectoring.h b/src/main/flight/thrust_vectoring.h
new file mode 100644
index 00000000000..c44797280af
--- /dev/null
+++ b/src/main/flight/thrust_vectoring.h
@@ -0,0 +1,46 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include "config/parameter_group.h"
+
+// Thrust vectoring: dedicated servo mixer input sources (INPUT_TVC_*) that
+// carry the same stabilized commands as the control surfaces but with a
+// thrust dependent gain. The torque a vectoring vane / tilting motor can
+// produce scales with thrust, so the deflection is compensated inversely
+// (capped at low thrust) to keep the control loop gain roughly constant --
+// full authority in a prop hang, no overcontrol at full power.
+
+typedef struct thrustVectoringConfig_s {
+    uint16_t gain;          // % overall TVC deflection gain at full thrust
+    uint8_t  thrustComp;    // % inverse thrust compensation: 0 = none, 100 = full 1/thrust
+} thrustVectoringConfig_t;
+
+PG_DECLARE(thrustVectoringConfig_t, thrustVectoringConfig);
+
+// Combined TVC gain for the current thrust fraction [0..1]
+float thrustVectoringGain(float thrustFraction);
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index e50115d99ed..b9f56c36d1f 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -132,4 +132,8 @@
 #define MSP2_INAV_SET_WP_INDEX                  0x2221  //in message  jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start)
 #define MSP2_INAV_SET_CRUISE_HEADING            0x2223  //in message  set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999)
 
-#define MSP2_INAV_SET_AUX_RC                    0x2230
\ No newline at end of file
+#define MSP2_INAV_SET_AUX_RC                    0x2230
+
+#define MSP2_INAV_FIGURE_SEQUENCE               0x2240
+#define MSP2_INAV_SET_FIGURE_SEQUENCE           0x2241
+#define MSP2_INAV_ORIENTATION_HOLD_TEST         0x2242  //in/out: level-1 test injection, 8x float32 (q_est wxyz, q_target wxyz) -> 6x float32 (err_deg xyz, rate_target_dps xyz); pure computation
\ No newline at end of file
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index 67487606da0..5daa102d8ce 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -57,6 +57,7 @@ typedef enum {
 } mspPendingSystemRequest_e;
 
 #define MSP_PORT_INBUF_SIZE 192
+#ifndef MSP_PORT_OUTBUF_SIZE
 #ifdef USE_FLASHFS
 #define MSP_PORT_DATAFLASH_BUFFER_SIZE 4096
 #define MSP_PORT_DATAFLASH_INFO_SIZE 16
@@ -64,6 +65,7 @@ typedef enum {
 #else
 #define MSP_PORT_OUTBUF_SIZE 512
 #endif
+#endif
 
 typedef struct __attribute__((packed)) {
     uint8_t size;
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index eda064bd68b..641b92aee52 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -4761,6 +4761,11 @@ bool navigationPositionEstimateIsHealthy(void)
     return posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE && STATE(GPS_FIX_HOME);
 }
 
+bool navIsAltitudeEstimateTrusted(void)
+{
+    return posControl.flags.estAltStatus >= EST_USABLE;
+}
+
 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
 {
     const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) ||
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index eb1621e9f8d..3e9b21357ad 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -683,6 +683,7 @@ int8_t navigationGetHeadingControlState(void);
 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass);
 bool navigationPositionEstimateIsHealthy(void);
 bool navIsCalibrationComplete(void);
+bool navIsAltitudeEstimateTrusted(void);
 bool navigationTerrainFollowingEnabled(void);
 
 /* Access to estimated position and velocity */
diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h
index 46ad6d9dad4..eb4b97c7bfd 100644
--- a/src/main/target/SITL/target.h
+++ b/src/main/target/SITL/target.h
@@ -63,6 +63,7 @@
 #define USE_MAG
 #define USE_BARO
 #define USE_PITOT_FAKE
+#define MSP_PORT_OUTBUF_SIZE 1024   // no FLASHFS on SITL; the full box-name list exceeds 512
 #define USE_IMU_FAKE
 #define USE_FAKE_BARO
 #define USE_FAKE_MAG
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 6990cfd4415..1e2e72f0f15 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -74,6 +74,8 @@
 #define USE_SMITH_PREDICTOR
 #define USE_RATE_DYNAMICS
 #define USE_EXTENDED_CMS_MENUS
+#define USE_ORIENTATION_HOLD
+#define USE_THRUST_VECTORING
 
 // Allow default rangefinders
 #define USE_RANGEFINDER