From 41a81e4ba0533298ed4fb53aea459109ea028350 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 5 Jan 2021 12:03:23 +0100
Subject: [PATCH 1/9] Init new PID controller

---
 src/main/fc/fc_msp_box.c  |  4 ++++
 src/main/fc/rc_modes.h    |  1 +
 src/main/fc/settings.yaml | 12 ++++++++++++
 src/main/flight/pid.c     | 29 ++++++++++++++++++++++++++++-
 src/main/flight/pid.h     |  3 +++
 5 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 3afe82c9cbc..4f9687f3b94 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -86,6 +86,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXUSER2, "USER2", BOX_PERMANENT_ID_USER2 },
     { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
     { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
+    { BOXAUTOLEVEL, "AUTO LEVEL", 51 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -239,6 +240,9 @@ void initActiveBoxIds(void)
 #if defined(USE_AUTOTUNE_FIXED_WING)
         activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
 #endif
+        if (sensors(SENSOR_BARO)) {
+            activeBoxIds[activeBoxIdCount++] = BOXAUTOLEVEL;
+        }
     }
 
     /*
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 2058c822d31..cee9f9ddac6 100755
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -68,6 +68,7 @@ typedef enum {
     BOXFPVANGLEMIX   = 39,
     BOXLOITERDIRCHN  = 40,
     BOXMSPRCOVERRIDE = 41,
+    BOXAUTOLEVEL     = 42,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 05f6d25575e..5992ac008b7 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1872,6 +1872,18 @@ groups:
         condition: USE_GYRO_KALMAN
         min: 1
         max: 16000
+      - name: fw_level_pitch_trim
+        description: "Pitch trim for self-leveling flight modes. In degrees"
+        default_value: "0"
+        field: fixedWingLevelTrim
+        min: -10
+        max: 10
+      - name: fw_level_pitch_gain
+        description: "I-gain for the pitch trim for self-leveling flight modes."
+        default_value: "0"
+        field: fixedWingLevelTrimGain
+        min: 0
+        max: 1000
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 5e5f9d3b5a6..3574a602854 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -154,6 +154,9 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
 
+static EXTENDED_FASTRAM float fixedWingLevelTrim;
+static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
+
 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
@@ -279,6 +282,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .kalman_w = 4,
         .kalman_sharpness = 100,
         .kalmanEnabled = 0,
+        .fixedWingLevelTrim = 0,
+        .fixedWingLevelTrimGain = 100,
 );
 
 bool pidInitFilters(void)
@@ -530,9 +535,19 @@ 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())
+    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);
 
+        DEBUG_SET(DEBUG_ALWAYS, 0, angleTarget * 10);
+        DEBUG_SET(DEBUG_ALWAYS, 1, fixedWingLevelTrim * 10);
+        DEBUG_SET(DEBUG_ALWAYS, 2, getEstimatedActualVelocity(Z));
+
+        //Apply level trim
+        angleTarget -= fixedWingLevelTrim;
+        DEBUG_SET(DEBUG_ALWAYS, 3, angleTarget * 10);
+        
+    }
+
     const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
 
     float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
@@ -1107,6 +1122,18 @@ void pidInit(void)
         gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness);
     }
 #endif
+
+    fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim;
+
+    navPidInit(
+        &fixedWingLevelTrimController,
+        0.0f,
+        pidProfile()->fixedWingLevelTrimGain / 1000.0f,
+        0.0f,
+        0.0f,
+        2.0f
+    );
+
 }
 
 const pidBank_t * pidBank(void) { 
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 7f83accab6c..fefe8e41674 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -150,6 +150,9 @@ typedef struct pidProfile_s {
     uint16_t kalman_w;
     uint16_t kalman_sharpness;
     uint8_t kalmanEnabled;
+
+    float fixedWingLevelTrim;
+    float fixedWingLevelTrimGain;
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {

From 006c21e5245810ca16e7499e927a401672787d1d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 5 Jan 2021 12:23:48 +0100
Subject: [PATCH 2/9] Extract navigation PID controller to separate module

---
 src/main/CMakeLists.txt                  |   2 +
 src/main/common/fp_pid.c                 | 156 +++++++++++++++++++++++
 src/main/common/fp_pid.h                 |  75 +++++++++++
 src/main/fc/cli.c                        |   1 +
 src/main/flight/pid.c                    |   1 +
 src/main/navigation/navigation.c         | 126 ------------------
 src/main/navigation/navigation.h         |  28 +---
 src/main/navigation/navigation_private.h |  22 ----
 src/main/programming/pid.h               |   2 +-
 9 files changed, 237 insertions(+), 176 deletions(-)
 create mode 100644 src/main/common/fp_pid.c
 create mode 100644 src/main/common/fp_pid.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index e0c50535eb4..211384312a7 100644
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -24,6 +24,8 @@ main_sources(COMMON_SRC
     common/encoding.h
     common/filter.c
     common/filter.h
+    common/fp_pid.c
+    common/fp_pid.h
     common/gps_conversion.c
     common/gps_conversion.h
     common/log.c
diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c
new file mode 100644
index 00000000000..753789c9dc1
--- /dev/null
+++ b/src/main/common/fp_pid.c
@@ -0,0 +1,156 @@
+/*
+ * 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"
+
+FILE_COMPILE_FOR_SPEED
+
+#include <math.h>
+#include "common/fp_pid.h"
+
+/*-----------------------------------------------------------
+ * Float point PID-controller implementation
+ *-----------------------------------------------------------*/
+// Implementation of PID with back-calculation I-term anti-windup
+// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
+// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
+float navPidApply3(
+    pidController_t *pid,
+    const float setpoint,
+    const float measurement,
+    const float dt,
+    const float outMin,
+    const float outMax,
+    const pidControllerFlags_e pidFlags,
+    const float gainScaler,
+    const float dTermScaler
+) {
+    float newProportional, newDerivative, newFeedForward;
+    float error = setpoint - measurement;
+
+    /* P-term */
+    newProportional = error * pid->param.kP * gainScaler;
+
+    /* D-term */
+    if (pid->reset) {
+        pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement;
+        pid->reset = false;
+    }
+
+    if (pidFlags & PID_DTERM_FROM_ERROR) {
+        /* Error-tracking D-term */
+        newDerivative = (error - pid->last_input) / dt;
+        pid->last_input = error;
+    } else {
+        /* Measurement tracking D-term */
+        newDerivative = -(measurement - pid->last_input) / dt;
+        pid->last_input = measurement;
+    }
+
+    if (pid->dTermLpfHz > 0) {
+        newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
+    } else {
+        newDerivative = pid->param.kD * newDerivative;
+    }
+
+    newDerivative = newDerivative * gainScaler * dTermScaler;
+
+    if (pidFlags & PID_ZERO_INTEGRATOR) {
+        pid->integrator = 0.0f;
+    }
+
+    /*
+     * Compute FeedForward parameter
+     */
+    newFeedForward = setpoint * pid->param.kFF * gainScaler;
+
+    /* Pre-calculate output and limit it if actuator is saturating */
+    const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
+    const float outValConstrained = constrainf(outVal, outMin, outMax);
+
+    pid->proportional = newProportional;
+    pid->integral = pid->integrator;
+    pid->derivative = newDerivative;
+    pid->feedForward = newFeedForward;
+    pid->output_constrained = outValConstrained;
+
+    /* Update I-term */
+    if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
+        const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
+
+        if (pidFlags & PID_SHRINK_INTEGRATOR) {
+            // Only allow integrator to shrink
+            if (fabsf(newIntegrator) < fabsf(pid->integrator)) {
+                pid->integrator = newIntegrator;
+            }
+        }
+        else {
+            pid->integrator = newIntegrator;
+        }
+    }
+    
+    if (pidFlags & PID_LIMIT_INTEGRATOR) {
+        pid->integrator = constrainf(pid->integrator, outMin, outMax);
+    } 
+
+    return outValConstrained;
+}
+
+float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
+{
+    return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f);
+}
+
+void navPidReset(pidController_t *pid)
+{
+    pid->reset = true;
+    pid->proportional = 0.0f;
+    pid->integral = 0.0f;
+    pid->derivative = 0.0f;
+    pid->integrator = 0.0f;
+    pid->last_input = 0.0f;
+    pid->feedForward = 0.0f;
+    pt1FilterReset(&pid->dterm_filter_state, 0.0f);
+    pid->output_constrained = 0.0f;
+}
+
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz)
+{
+    pid->param.kP = _kP;
+    pid->param.kI = _kI;
+    pid->param.kD = _kD;
+    pid->param.kFF = _kFF;
+
+    if (_kI > 1e-6f && _kP > 1e-6f) {
+        float Ti = _kP / _kI;
+        float Td = _kD / _kP;
+        pid->param.kT = 2.0f / (Ti + Td);
+    }
+    else {
+        pid->param.kI = 0.0;
+        pid->param.kT = 0.0;
+    }
+    pid->dTermLpfHz = _dTermLpfHz;
+    navPidReset(pid);
+}
\ No newline at end of file
diff --git a/src/main/common/fp_pid.h b/src/main/common/fp_pid.h
new file mode 100644
index 00000000000..0698c94ddbc
--- /dev/null
+++ b/src/main/common/fp_pid.h
@@ -0,0 +1,75 @@
+/*
+ * 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 "common/filter.h"
+#include "common/maths.h"
+
+typedef struct {
+    float kP;
+    float kI;
+    float kD;
+    float kT;   // Tracking gain (anti-windup)
+    float kFF;  // FeedForward Component
+} pidControllerParam_t;
+
+typedef enum {
+    PID_DTERM_FROM_ERROR            = 1 << 0,
+    PID_ZERO_INTEGRATOR             = 1 << 1,
+    PID_SHRINK_INTEGRATOR           = 1 << 2,
+    PID_LIMIT_INTEGRATOR            = 1 << 3,
+} pidControllerFlags_e;
+
+typedef struct {
+    bool reset;
+    pidControllerParam_t param;
+    pt1Filter_t dterm_filter_state;     // last derivative for low-pass filter
+    float dTermLpfHz;                   // dTerm low pass filter cutoff frequency
+    float integrator;                   // integrator value
+    float last_input;                   // last input for derivative
+
+    float integral;                     // used integral value in output
+    float proportional;                 // used proportional value in output
+    float derivative;                   // used derivative value in output
+    float feedForward;                  // used FeedForward value in output
+    float output_constrained;           // controller output constrained
+} pidController_t;
+
+float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
+float navPidApply3(
+    pidController_t *pid,
+    const float setpoint,
+    const float measurement,
+    const float dt,
+    const float outMin,
+    const float outMax,
+    const pidControllerFlags_e pidFlags,
+    const float gainScaler,
+    const float dTermScaler
+);
+void navPidReset(pidController_t *pid);
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);
\ No newline at end of file
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index e3b147d4849..48e05a86556 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -41,6 +41,7 @@ uint8_t cliMode = 0;
 #include "common/memory.h"
 #include "common/time.h"
 #include "common/typeconversion.h"
+#include "common/fp_pid.h"
 #include "programming/global_variables.h"
 #include "programming/pid.h"
 
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 3574a602854..0e627da696f 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -30,6 +30,7 @@ FILE_COMPILE_FOR_SPEED
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
+#include "common/fp_pid.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index b00dbbc0500..e61811104c6 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1913,132 +1913,6 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
     return &posControl.rthState.homeTmpWaypoint;
 }
 
-/*-----------------------------------------------------------
- * Float point PID-controller implementation
- *-----------------------------------------------------------*/
-// Implementation of PID with back-calculation I-term anti-windup
-// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
-// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
-float navPidApply3(
-    pidController_t *pid,
-    const float setpoint,
-    const float measurement,
-    const float dt,
-    const float outMin,
-    const float outMax,
-    const pidControllerFlags_e pidFlags,
-    const float gainScaler,
-    const float dTermScaler
-) {
-    float newProportional, newDerivative, newFeedForward;
-    float error = setpoint - measurement;
-
-    /* P-term */
-    newProportional = error * pid->param.kP * gainScaler;
-
-    /* D-term */
-    if (pid->reset) {
-        pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement;
-        pid->reset = false;
-    }
-
-    if (pidFlags & PID_DTERM_FROM_ERROR) {
-        /* Error-tracking D-term */
-        newDerivative = (error - pid->last_input) / dt;
-        pid->last_input = error;
-    } else {
-        /* Measurement tracking D-term */
-        newDerivative = -(measurement - pid->last_input) / dt;
-        pid->last_input = measurement;
-    }
-
-    if (pid->dTermLpfHz > 0) {
-        newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
-    } else {
-        newDerivative = pid->param.kD * newDerivative;
-    }
-
-    newDerivative = newDerivative * gainScaler * dTermScaler;
-
-    if (pidFlags & PID_ZERO_INTEGRATOR) {
-        pid->integrator = 0.0f;
-    }
-
-    /*
-     * Compute FeedForward parameter
-     */
-    newFeedForward = setpoint * pid->param.kFF * gainScaler;
-
-    /* Pre-calculate output and limit it if actuator is saturating */
-    const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
-    const float outValConstrained = constrainf(outVal, outMin, outMax);
-
-    pid->proportional = newProportional;
-    pid->integral = pid->integrator;
-    pid->derivative = newDerivative;
-    pid->feedForward = newFeedForward;
-    pid->output_constrained = outValConstrained;
-
-    /* Update I-term */
-    if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
-        const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
-
-        if (pidFlags & PID_SHRINK_INTEGRATOR) {
-            // Only allow integrator to shrink
-            if (fabsf(newIntegrator) < fabsf(pid->integrator)) {
-                pid->integrator = newIntegrator;
-            }
-        }
-        else {
-            pid->integrator = newIntegrator;
-        }
-    }
-    
-    if (pidFlags & PID_LIMIT_INTEGRATOR) {
-        pid->integrator = constrainf(pid->integrator, outMin, outMax);
-    } 
-
-    return outValConstrained;
-}
-
-float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
-{
-    return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f);
-}
-
-void navPidReset(pidController_t *pid)
-{
-    pid->reset = true;
-    pid->proportional = 0.0f;
-    pid->integral = 0.0f;
-    pid->derivative = 0.0f;
-    pid->integrator = 0.0f;
-    pid->last_input = 0.0f;
-    pid->feedForward = 0.0f;
-    pt1FilterReset(&pid->dterm_filter_state, 0.0f);
-    pid->output_constrained = 0.0f;
-}
-
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz)
-{
-    pid->param.kP = _kP;
-    pid->param.kI = _kI;
-    pid->param.kD = _kD;
-    pid->param.kFF = _kFF;
-
-    if (_kI > 1e-6f && _kP > 1e-6f) {
-        float Ti = _kP / _kI;
-        float Td = _kD / _kP;
-        pid->param.kT = 2.0f / (Ti + Td);
-    }
-    else {
-        pid->param.kI = 0.0;
-        pid->param.kT = 0.0;
-    }
-    pid->dTermLpfHz = _dTermLpfHz;
-    navPidReset(pid);
-}
-
 /*-----------------------------------------------------------
  * Detects if thrust vector is facing downwards
  *-----------------------------------------------------------*/
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 5dcd8c71ca2..50ba6b2632f 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -23,6 +23,7 @@
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/vector.h"
+#include "common/fp_pid.h"
 
 #include "config/feature.h"
 
@@ -324,33 +325,6 @@ typedef struct navDestinationPath_s {
     int32_t bearing; // deg * 100
 } navDestinationPath_t;
 
-typedef struct {
-    float kP;
-    float kI;
-    float kD;
-    float kT;   // Tracking gain (anti-windup)
-    float kFF;  // FeedForward Component
-} pidControllerParam_t;
-
-typedef struct {
-    float kP;
-} pControllerParam_t;
-
-typedef struct {
-    bool reset;
-    pidControllerParam_t param;
-    pt1Filter_t dterm_filter_state;     // last derivative for low-pass filter
-    float dTermLpfHz;                   // dTerm low pass filter cutoff frequency
-    float integrator;                   // integrator value
-    float last_input;                   // last input for derivative
-
-    float integral;                     // used integral value in output
-    float proportional;                 // used proportional value in output
-    float derivative;                   // used derivative value in output
-    float feedForward;                  // used FeedForward value in output
-    float output_constrained;           // controller output constrained
-} pidController_t;
-
 typedef struct navigationPIDControllers_s {
     /* Multicopter PIDs */
     pidController_t   pos[XYZ_AXIS_COUNT];
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 95a67e39173..6e7c4d3c0c8 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -91,13 +91,6 @@ typedef struct navigationFlags_s {
     bool forcedRTHActivated;
 } navigationFlags_t;
 
-typedef enum {
-    PID_DTERM_FROM_ERROR            = 1 << 0,
-    PID_ZERO_INTEGRATOR             = 1 << 1,
-    PID_SHRINK_INTEGRATOR           = 1 << 2,
-    PID_LIMIT_INTEGRATOR            = 1 << 3,
-} pidControllerFlags_e;
-
 typedef struct {
     fpVector3_t pos;
     fpVector3_t vel;
@@ -392,21 +385,6 @@ extern multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 /* Internally used functions */
 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
 
-float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
-float navPidApply3(
-    pidController_t *pid,
-    const float setpoint,
-    const float measurement,
-    const float dt,
-    const float outMin,
-    const float outMax,
-    const pidControllerFlags_e pidFlags,
-    const float gainScaler,
-    const float dTermScaler
-);
-void navPidReset(pidController_t *pid);
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);
-
 bool isThrustFacingDownwards(void);
 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);
 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos);
diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h
index f120f9f0739..8645de8f130 100644
--- a/src/main/programming/pid.h
+++ b/src/main/programming/pid.h
@@ -26,11 +26,11 @@
 
 #include "config/parameter_group.h"
 #include "common/time.h"
+#include "common/fp_pid.h"
 
 #include "programming/logic_condition.h"
 #include "common/axis.h"
 #include "flight/pid.h"
-#include "navigation/navigation.h"
 
 #define MAX_PROGRAMMING_PID_COUNT 4
 

From ff46510b754ff546fa6b7e5439be9d4db7211053 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 10 Jan 2021 19:43:39 +0100
Subject: [PATCH 3/9] The first run on AutoLevel

---
 src/main/common/fp_pid.c  |  4 ++++
 src/main/common/fp_pid.h  |  1 +
 src/main/fc/fc_tasks.c    |  2 +-
 src/main/fc/settings.yaml |  2 +-
 src/main/flight/pid.c     | 25 +++++++++++++++++++++++--
 src/main/flight/pid.h     |  3 +++
 6 files changed, 33 insertions(+), 4 deletions(-)

diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c
index 753789c9dc1..84afe518ae7 100644
--- a/src/main/common/fp_pid.c
+++ b/src/main/common/fp_pid.c
@@ -147,6 +147,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_tasks.c b/src/main/fc/fc_tasks.c
index b000de70177..3dda55abb64 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -293,9 +293,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/settings.yaml b/src/main/fc/settings.yaml
index 5992ac008b7..b9f58f011fd 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1883,7 +1883,7 @@ groups:
         default_value: "0"
         field: fixedWingLevelTrimGain
         min: 0
-        max: 1000
+        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 0e627da696f..e7e716a0aca 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -284,7 +284,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .kalman_sharpness = 100,
         .kalmanEnabled = 0,
         .fixedWingLevelTrim = 0,
-        .fixedWingLevelTrimGain = 100,
+        .fixedWingLevelTrimGain = 10,
 );
 
 bool pidInitFilters(void)
@@ -1129,7 +1129,7 @@ void pidInit(void)
     navPidInit(
         &fixedWingLevelTrimController,
         0.0f,
-        pidProfile()->fixedWingLevelTrimGain / 1000.0f,
+        (float)pidProfile()->fixedWingLevelTrimGain / 100000.0f,
         0.0f,
         0.0f,
         2.0f
@@ -1152,3 +1152,24 @@ uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
         return &pidBank->pid[pidIndex].D;
     }
 }
+
+void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
+{
+    static timeUs_t previousUpdateTimeUs;
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
+
+    const float output = navPidApply3(
+        &fixedWingLevelTrimController,
+        0,
+        getEstimatedActualVelocity(Z),
+        dT,
+        -1000.0f,
+        1000.0f,
+        PID_LIMIT_INTEGRATOR,
+        1.0f,
+        1.0f
+    );
+
+    DEBUG_SET(DEBUG_ALWAYS, 4, output / 100);
+
+}
\ No newline at end of file
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index fefe8e41674..8d9a0238cb0 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
@@ -203,3 +204,5 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
 
 pidType_e pidIndexGetType(pidIndex_e pidIndex);
 uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);
+
+void updateFixedWingLevelTrim(timeUs_t currentTimeUs);

From 0a4498dacc77d7998f7512c5731420a889eebf10 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 3 Feb 2021 12:15:02 +0100
Subject: [PATCH 4/9] Refactor

---
 src/main/build/debug.h    |  1 +
 src/main/fc/settings.yaml |  2 +-
 src/main/flight/pid.c     | 30 ++++++++++++++++++++----------
 3 files changed, 22 insertions(+), 11 deletions(-)

diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index b484fd4d35c..a65690022bd 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -80,5 +80,6 @@ typedef enum {
     DEBUG_SPM_VARIO,    // Smartport master variometer
     DEBUG_PCF8574,
     DEBUG_DYNAMIC_GYRO_LPF,
+    DEBUG_AUTOLEVEL,
     DEBUG_COUNT
 } debugType_e;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index b9f58f011fd..787977ad076 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1873,7 +1873,7 @@ groups:
         min: 1
         max: 16000
       - name: fw_level_pitch_trim
-        description: "Pitch trim for self-leveling flight modes. In degrees"
+        description: "Pitch trim for self-leveling flight modes. In degrees. +5 means nose should be raised 5 deg from level"
         default_value: "0"
         field: fixedWingLevelTrim
         min: -10
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index e7e716a0aca..3f8b9cbaee4 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -537,16 +537,26 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
 
     // 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);
-
-        DEBUG_SET(DEBUG_ALWAYS, 0, angleTarget * 10);
-        DEBUG_SET(DEBUG_ALWAYS, 1, fixedWingLevelTrim * 10);
-        DEBUG_SET(DEBUG_ALWAYS, 2, getEstimatedActualVelocity(Z));
+        angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);        
+    }
 
-        //Apply level trim
-        angleTarget -= fixedWingLevelTrim;
-        DEBUG_SET(DEBUG_ALWAYS, 3, angleTarget * 10);
-        
+    //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);
     }
 
     const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
@@ -1170,6 +1180,6 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
         1.0f
     );
 
-    DEBUG_SET(DEBUG_ALWAYS, 4, output / 100);
+    DEBUG_SET(DEBUG_AUTOLEVEL, 4, output / 100);
 
 }
\ No newline at end of file

From fc7aeaf299de9421da9b820df63f63a0a265c123 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 18 Feb 2021 09:48:19 +0100
Subject: [PATCH 5/9] Autolevel implementation

---
 docs/Settings.md                 |  2 ++
 src/main/common/fp_pid.c         |  5 ++-
 src/main/fc/fc_msp_box.c         |  1 +
 src/main/fc/settings.yaml        | 10 ++++--
 src/main/flight/pid.c            | 53 +++++++++++++++++++++++++++++---
 src/main/flight/pid.h            |  3 ++
 src/main/navigation/navigation.c |  5 +++
 src/main/navigation/navigation.h |  1 +
 8 files changed, 72 insertions(+), 8 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index e0a8c522421..2906667c96d 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -122,6 +122,8 @@
 | fw_i_yaw | 10 | Fixed-wing rate stabilisation I-gain for YAW |
 | fw_iterm_limit_stick_position | 0.5 | 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 | 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 | Deadband for automatic self-trimming |
+| fw_level_pitch_gain | 10 | I-gain for the pitch trim for self-leveling flight modes. |
 | fw_level_pitch_trim | 0 | 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 | 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/common/fp_pid.c b/src/main/common/fp_pid.c
index 84afe518ae7..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) {
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 4f9687f3b94..a8f8438097b 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -367,6 +367,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/settings.yaml b/src/main/fc/settings.yaml
index f30c2487216..c3165231bcf 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -84,7 +84,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"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -1882,10 +1882,16 @@ groups:
         max: 10
       - name: fw_level_pitch_gain
         description: "I-gain for the pitch trim for self-leveling flight modes."
-        default_value: "0"
+        default_value: "10"
         field: fixedWingLevelTrimGain
         min: 0
         max: 20
+      - name: fw_level_pitch_deadband
+        description: "Deadband for automatic self-trimming"
+        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 722b5fa52c6..a2e7468a82a 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -155,6 +155,11 @@ 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;
 
@@ -285,6 +290,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .kalmanEnabled = 0,
         .fixedWingLevelTrim = 0,
         .fixedWingLevelTrimGain = 10,
+        .fixedWingLevelTrimDeadband = FIXED_WING_LEVEL_TRIM_DEADBAND_DEFAULT,
 );
 
 bool pidInitFilters(void)
@@ -1180,21 +1186,58 @@ uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
 
 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,
+        0,  //Setpoint is always 0 as we try to keep level flight
         getEstimatedActualVelocity(Z),
         dT,
-        -1000.0f,
-        1000.0f,
-        PID_LIMIT_INTEGRATOR,
+        -FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
+        FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
+        flags,
         1.0f,
         1.0f
     );
 
-    DEBUG_SET(DEBUG_AUTOLEVEL, 4, output / 100);
+    DEBUG_SET(DEBUG_AUTOLEVEL, 4, output);
+    fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);
 
+    previousArmingState = !!ARMING_FLAG(ARMED);
 }
\ No newline at end of file
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 8d9a0238cb0..d100f57b503 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -55,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 {
@@ -154,6 +156,7 @@ typedef struct pidProfile_s {
 
     float fixedWingLevelTrim;
     float fixedWingLevelTrimGain;
+    float fixedWingLevelTrimDeadband;
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 01e83e70b79..c0d1b97c1c4 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3586,6 +3586,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 50ba6b2632f..2afb311a0f2 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -495,6 +495,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.
  */

From 26ca6849bf416bed65cd2e3663e1e984fb3d7ba6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 2 Mar 2021 09:50:07 +0100
Subject: [PATCH 6/9] Lower default fixedWingLevelTrimGain

---
 src/main/flight/pid.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index a2e7468a82a..6682a6d9de2 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -289,7 +289,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .kalman_sharpness = 100,
         .kalmanEnabled = 0,
         .fixedWingLevelTrim = 0,
-        .fixedWingLevelTrimGain = 10,
+        .fixedWingLevelTrimGain = 5,
         .fixedWingLevelTrimDeadband = FIXED_WING_LEVEL_TRIM_DEADBAND_DEFAULT,
 );
 

From 9cbecaa45fb01515dfce035801768a050c2e8db7 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 2 Mar 2021 09:53:34 +0100
Subject: [PATCH 7/9] Update docs

---
 docs/Settings.md          | 4 ++--
 src/main/fc/settings.yaml | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 2906667c96d..7f35d2a3aaf 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -122,8 +122,8 @@
 | fw_i_yaw | 10 | Fixed-wing rate stabilisation I-gain for YAW |
 | fw_iterm_limit_stick_position | 0.5 | 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 | 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 | Deadband for automatic self-trimming |
-| fw_level_pitch_gain | 10 | I-gain for the pitch trim for self-leveling flight modes. |
+| fw_level_pitch_deadband | 5 | Deadband for automatic leveling when AUTOLEVEL mode is used. |
+| fw_level_pitch_gain | 5 | I-gain for the pitch trim for self-leveling flight modes. |
 | fw_level_pitch_trim | 0 | 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 | 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/fc/settings.yaml b/src/main/fc/settings.yaml
index c3165231bcf..01bfaf65857 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1882,12 +1882,12 @@ groups:
         max: 10
       - name: fw_level_pitch_gain
         description: "I-gain for the pitch trim for self-leveling flight modes."
-        default_value: "10"
+        default_value: "5"
         field: fixedWingLevelTrimGain
         min: 0
         max: 20
       - name: fw_level_pitch_deadband
-        description: "Deadband for automatic self-trimming"
+        description: "Deadband for automatic leveling when AUTOLEVEL mode is used."
         default_value: "5"
         field: fixedWingLevelTrimDeadband
         min: 0

From 843558fcfaaf93e1e669009e7fcc29e8ddb38a7b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 13 Mar 2021 17:56:02 +0100
Subject: [PATCH 8/9] Minor doc change to satisfy GH checks

---
 docs/Settings.md          | 2 +-
 src/main/fc/settings.yaml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 8f71ee12e3c..cf97f6720af 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -127,7 +127,7 @@
 | fw_iterm_limit_stick_position | 0.5 | 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 | 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 | Deadband for automatic leveling when AUTOLEVEL mode is used. |
-| fw_level_pitch_gain | 5 | I-gain for the pitch trim for self-leveling flight modes. |
+| fw_level_pitch_gain | 5 | 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 | 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 | 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/fc/settings.yaml b/src/main/fc/settings.yaml
index db1b0ca3afa..ac0aa994b31 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1910,7 +1910,7 @@ groups:
         min: -10
         max: 10
       - name: fw_level_pitch_gain
-        description: "I-gain for the pitch trim for self-leveling flight modes."
+        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

From 860abf6427f0a0b11d387aa2263b7140d5976267 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 11 May 2021 15:03:30 +0200
Subject: [PATCH 9/9] Docs update

---
 docs/Settings.md | 2 ++
 1 file changed, 2 insertions(+)

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) |