diff --git a/docs/Settings.md b/docs/Settings.md
index 22ad622c926..2da1ed9327f 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -129,6 +129,8 @@
 | fw_i_yaw | 10 | 0 | 200 | Fixed-wing rate stabilisation I-gain for YAW |
 | fw_iterm_limit_stick_position | 0.5 | 0 | 1 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side |
 | fw_iterm_throw_limit | 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. |
+| fw_level_pitch_deadband | 5 | 0 | 20 | Deadband for automatic leveling when AUTOLEVEL mode is used. |
+| fw_level_pitch_gain | 5 | 0 | 20 | I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations |
 | fw_level_pitch_trim | 0 | -10 | 10 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level |
 | fw_loiter_direction | RIGHT |  |  | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. |
 | fw_min_throttle_down_pitch | 0 | 0 | 450 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) |
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index c4843e76fa9..76f4cc5836b 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -80,6 +80,7 @@ typedef enum {
     DEBUG_SPM_VARIO,    // Smartport master variometer
     DEBUG_PCF8574,
     DEBUG_DYNAMIC_GYRO_LPF,
+    DEBUG_AUTOLEVEL,
     DEBUG_FW_D,
     DEBUG_IMU2,
     DEBUG_ALTITUDE,
diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c
index 753789c9dc1..346c6ace3fc 100644
--- a/src/main/common/fp_pid.c
+++ b/src/main/common/fp_pid.c
@@ -96,7 +96,10 @@ float navPidApply3(
     pid->output_constrained = outValConstrained;
 
     /* Update I-term */
-    if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
+    if (
+        !(pidFlags & PID_ZERO_INTEGRATOR) &&
+        !(pidFlags & PID_FREEZE_INTEGRATOR) 
+    ) {
         const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
 
         if (pidFlags & PID_SHRINK_INTEGRATOR) {
@@ -147,6 +150,10 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
         float Td = _kD / _kP;
         pid->param.kT = 2.0f / (Ti + Td);
     }
+    else if (_kI > 1e-6f) {
+        pid->param.kI = _kI;
+        pid->param.kT = 0.0f;
+    }
     else {
         pid->param.kI = 0.0;
         pid->param.kT = 0.0;
diff --git a/src/main/common/fp_pid.h b/src/main/common/fp_pid.h
index 0698c94ddbc..17a43bd9209 100644
--- a/src/main/common/fp_pid.h
+++ b/src/main/common/fp_pid.h
@@ -42,6 +42,7 @@ typedef enum {
     PID_ZERO_INTEGRATOR             = 1 << 1,
     PID_SHRINK_INTEGRATOR           = 1 << 2,
     PID_LIMIT_INTEGRATOR            = 1 << 3,
+    PID_FREEZE_INTEGRATOR           = 1 << 4,
 } pidControllerFlags_e;
 
 typedef struct {
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 1bd0b6e8e01..5ec8491e52f 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -91,6 +91,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXPREARM, "PREARM", 51 },
     { BOXTURTLE, "TURTLE", 52 },
     { BOXNAVCRUISE, "NAV CRUISE", 53 },
+    { BOXAUTOLEVEL, "AUTO LEVEL", 54 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -250,6 +251,9 @@ void initActiveBoxIds(void)
 #if defined(USE_AUTOTUNE_FIXED_WING)
         activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
 #endif
+        if (sensors(SENSOR_BARO)) {
+            activeBoxIds[activeBoxIdCount++] = BOXAUTOLEVEL;
+        }
     }
 
     /*
@@ -380,6 +384,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)),   BOXMSPRCOVERRIDE);
 #endif
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)),       BOXAUTOLEVEL);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index d7a073bf647..e4ef2a21325 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -304,9 +304,9 @@ void taskUpdateOsd(timeUs_t currentTimeUs)
 
 void taskUpdateAux(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
     updatePIDCoefficients();
     dynamicLpfGyroTask();
+    updateFixedWingLevelTrim(currentTimeUs);
 }
 
 void fcTasksInit(void)
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 506ab8f9b3b..9c47866cab8 100755
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -71,6 +71,7 @@ typedef enum {
     BOXPREARM        = 42,
     BOXTURTLE        = 43,
     BOXNAVCRUISE     = 44,
+    BOXAUTOLEVEL     = 45,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a7156d480d7..b1b6120fb3f 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -93,7 +93,7 @@ tables:
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
-      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE",
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "FW_D", "IMU2", "ALTITUDE",
       "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
@@ -2112,6 +2112,18 @@ groups:
         condition: USE_SMITH_PREDICTOR
         min: 1
         max: 500
+      - name: fw_level_pitch_gain
+        description: "I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations"
+        default_value: 5
+        field: fixedWingLevelTrimGain
+        min: 0
+        max: 20
+      - name: fw_level_pitch_deadband
+        description: "Deadband for automatic leveling when AUTOLEVEL mode is used."
+        default_value: 5
+        field: fixedWingLevelTrimDeadband
+        min: 0
+        max: 20
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 3f1b9ad6de7..6f6222d9016 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -160,7 +160,14 @@ static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
+
+#define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand
+#define FIXED_WING_LEVEL_TRIM_DIVIDER 500.0f
+#define FIXED_WING_LEVEL_TRIM_MULTIPLIER 1.0f / FIXED_WING_LEVEL_TRIM_DIVIDER
+#define FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT FIXED_WING_LEVEL_TRIM_DIVIDER * FIXED_WING_LEVEL_TRIM_MAX_ANGLE
+
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
+static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2);
 
@@ -300,6 +307,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 #endif
 
         .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
+        .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
+        .fixedWingLevelTrimDeadband = SETTING_FW_LEVEL_PITCH_DEADBAND_DEFAULT,
 
 #ifdef USE_SMITH_PREDICTOR
         .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
@@ -597,8 +606,28 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
     float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
 
     // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
-    if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
-        angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
+    if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
+        angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);        
+    }
+
+    //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
+    if (axis == FD_PITCH && STATE(AIRPLANE)) {
+        DEBUG_SET(DEBUG_AUTOLEVEL, 0, angleTarget * 10);
+        DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
+        DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
+
+        /* 
+         * fixedWingLevelTrim has opposite sign to rcCommand.
+         * Positive rcCommand means nose should point downwards
+         * Negative rcCommand mean nose should point upwards
+         * This is counter intuitive and a natural way suggests that + should mean UP
+         * This is why fixedWingLevelTrim has opposite sign to rcCommand
+         * Positive fixedWingLevelTrim means nose should point upwards
+         * Negative fixedWingLevelTrim means nose should point downwards
+         */
+        angleTarget -= fixedWingLevelTrim;   
+        DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
+    }
 
         //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
     if (axis == FD_PITCH && STATE(AIRPLANE)) {
@@ -1250,6 +1279,16 @@ void pidInit(void)
 #endif
 
     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim;
+
+    navPidInit(
+        &fixedWingLevelTrimController,
+        0.0f,
+        (float)pidProfile()->fixedWingLevelTrimGain / 100000.0f,
+        0.0f,
+        0.0f,
+        2.0f
+    );
+
 }
 
 const pidBank_t * pidBank(void) { 
@@ -1258,3 +1297,61 @@ const pidBank_t * pidBank(void) {
 pidBank_t * pidBankMutable(void) { 
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }
+
+void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
+{
+    if (!STATE(AIRPLANE)) {
+        return;
+    }
+
+    static timeUs_t previousUpdateTimeUs;
+    static bool previousArmingState;
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
+
+    /*
+     * On every ARM reset the controller
+     */
+    if (ARMING_FLAG(ARMED) && !previousArmingState) {
+        navPidReset(&fixedWingLevelTrimController);
+    } 
+
+    /*
+     * On disarm update the default value
+     */
+    if (!ARMING_FLAG(ARMED) && previousArmingState) {
+        pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
+    } 
+
+    /*
+     * Prepare flags for the PID controller
+     */
+    pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
+
+    //Iterm should freeze when pitch stick is deflected
+    if (
+        !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) ||
+        rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) ||
+        rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband) ||
+        (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) ||
+        navigationIsControllingAltitude()
+    ) {
+        flags |= PID_FREEZE_INTEGRATOR;
+    }
+
+    const float output = navPidApply3(
+        &fixedWingLevelTrimController,
+        0,  //Setpoint is always 0 as we try to keep level flight
+        getEstimatedActualVelocity(Z),
+        dT,
+        -FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
+        FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
+        flags,
+        1.0f,
+        1.0f
+    );
+
+    DEBUG_SET(DEBUG_AUTOLEVEL, 4, output);
+    fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);
+
+    previousArmingState = !!ARMING_FLAG(ARMED);
+}
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 357f9c07dca..0d4719acb80 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -19,6 +19,7 @@
 
 #include "config/parameter_group.h"
 #include "fc/runtime_config.h"
+#include "common/time.h"
 
 #define GYRO_SATURATION_LIMIT       1800        // 1800dps
 #define PID_SUM_LIMIT_MIN           100
@@ -54,6 +55,8 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
 
 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
 
+#define FIXED_WING_LEVEL_TRIM_DEADBAND_DEFAULT 5
+
 #define TASK_AUX_RATE_HZ   100 //In Hz
 
 typedef enum {
@@ -161,6 +164,8 @@ typedef struct pidProfile_s {
 #endif
 
     float fixedWingLevelTrim;
+    float fixedWingLevelTrimGain;
+    float fixedWingLevelTrimDeadband;
 #ifdef USE_SMITH_PREDICTOR
     float smithPredictorStrength;
     float smithPredictorDelay;
@@ -226,3 +231,5 @@ void autotuneUpdateState(void);
 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
 
 pidType_e pidIndexGetType(pidIndex_e pidIndex);
+
+void updateFixedWingLevelTrim(timeUs_t currentTimeUs);
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index fd7e0c90d30..e8f70c6e7d7 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3648,6 +3648,11 @@ bool navigationIsControllingThrottle(void)
     return navigationInAutomaticThrottleMode() && (getMotorStatus() != MOTOR_STOPPED_USER);
 }
 
+bool navigationIsControllingAltitude(void) {
+    navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
+    return (stateFlags & NAV_CTL_ALT);
+}
+
 bool navigationIsFlyingAutonomousMode(void)
 {
     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 1809226452f..64995419214 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -524,6 +524,7 @@ bool navigationIsControllingThrottle(void);
 bool isFixedWingAutoThrottleManuallyIncreased(void);
 bool navigationIsFlyingAutonomousMode(void);
 bool navigationIsExecutingAnEmergencyLanding(void);
+bool navigationIsControllingAltitude(void);
 /* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS
  * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active.
  */