From baa834fcee49598ac74158ddad85ee54e2dab02c Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@gmail.com>
Date: Sun, 4 Oct 2020 18:50:03 +0200
Subject: [PATCH 01/50] Add driver for BMI088 IMU

---
 src/main/CMakeLists.txt                   |   2 +
 src/main/drivers/accgyro/accgyro_bmi088.c | 245 ++++++++++++++++++++++
 src/main/drivers/accgyro/accgyro_bmi088.h |  30 +++
 src/main/drivers/bus.h                    |   2 +
 src/main/fc/settings.yaml                 |   2 +-
 src/main/sensors/acceleration.c           |  14 ++
 src/main/sensors/acceleration.h           |   3 +-
 src/main/sensors/gyro.c                   |  10 +
 src/main/sensors/gyro.h                   |   1 +
 src/main/target/common_hardware.c         |   9 +
 10 files changed, 316 insertions(+), 2 deletions(-)
 create mode 100644 src/main/drivers/accgyro/accgyro_bmi088.c
 create mode 100644 src/main/drivers/accgyro/accgyro_bmi088.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 674b766d0ba..018f1604b81 100644
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -71,6 +71,8 @@ main_sources(COMMON_SRC
     drivers/accgyro/accgyro_adxl345.h
     drivers/accgyro/accgyro_bma280.c
     drivers/accgyro/accgyro_bma280.h
+    drivers/accgyro/accgyro_bmi088.c
+    drivers/accgyro/accgyro_bmi088.h
     drivers/accgyro/accgyro_bmi160.c
     drivers/accgyro/accgyro_bmi160.h
     drivers/accgyro/accgyro_fake.c
diff --git a/src/main/drivers/accgyro/accgyro_bmi088.c b/src/main/drivers/accgyro/accgyro_bmi088.c
new file mode 100644
index 00000000000..abe89af1df2
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_bmi088.c
@@ -0,0 +1,245 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 <stdint.h>
+
+#include "platform.h"
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "drivers/system.h"
+#include "drivers/time.h"
+#include "drivers/io.h"
+#include "drivers/exti.h"
+#include "drivers/bus.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_bmi088.h"
+
+
+#if defined(USE_IMU_BMI088)
+
+/*
+  device registers, names follow datasheet conventions, with REGA_
+  prefix for accel, and REGG_ prefix for gyro
+ */
+#define REGA_CHIPID        0x00
+#define REGA_ERR_REG       0x02
+#define REGA_STATUS        0x03
+#define REGA_X_LSB         0x12
+#define REGA_INT_STATUS_1  0x1D
+#define REGA_TEMP_LSB      0x22
+#define REGA_TEMP_MSB      0x23
+#define REGA_CONF          0x40
+#define REGA_RANGE         0x41
+#define REGA_PWR_CONF      0x7C
+#define REGA_PWR_CTRL      0x7D
+#define REGA_SOFTRESET     0x7E
+#define REGA_FIFO_CONFIG0  0x48
+#define REGA_FIFO_CONFIG1  0x49
+#define REGA_FIFO_DOWNS    0x45
+#define REGA_FIFO_DATA     0x26
+#define REGA_FIFO_LEN0     0x24
+#define REGA_FIFO_LEN1     0x25
+
+#define REGG_CHIPID        0x00
+#define REGG_RATE_X_LSB    0x02
+#define REGG_INT_CTRL      0x15
+#define REGG_INT_STATUS_1  0x0A
+#define REGG_INT_STATUS_2  0x0B
+#define REGG_INT_STATUS_3  0x0C
+#define REGG_FIFO_STATUS   0x0E
+#define REGG_RANGE         0x0F
+#define REGG_BW            0x10
+#define REGG_LPM1          0x11
+#define REGG_RATE_HBW      0x13
+#define REGG_BGW_SOFTRESET 0x14
+#define REGG_FIFO_CONFIG_1 0x3E
+#define REGG_FIFO_DATA     0x3F
+
+
+static void bmi088GyroInit(gyroDev_t *gyro)
+{
+    gyroIntExtiInit(gyro);
+
+    busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION);
+
+    // Soft reset
+    busWrite(gyro->busDev, REGG_BGW_SOFTRESET, 0xB6);
+    delay(100);
+
+    // ODR 2kHz, BW 532Hz
+    busWrite(gyro->busDev, REGG_BW, 0x81);
+    delay(1);
+
+    // Enable sampling
+    busWrite(gyro->busDev, REGG_INT_CTRL, 0x80);
+    delay(1);
+
+    busSetSpeed(gyro->busDev, BUS_SPEED_FAST);
+}
+
+static void bmi088AccInit(accDev_t *acc)
+{
+    busSetSpeed(acc->busDev, BUS_SPEED_INITIALIZATION);
+
+    // Soft reset
+    busWrite(acc->busDev, REGA_SOFTRESET, 0xB6);
+    delay(100);
+
+    // Active mode
+    busWrite(acc->busDev, REGA_PWR_CONF, 0);
+    delay(100);
+
+    // ACC ON
+    busWrite(acc->busDev, REGA_PWR_CTRL, 0x04);
+    delay(100);
+
+    // OSR4, ODR 1600Hz
+    busWrite(acc->busDev, REGA_CONF, 0x8C);
+    delay(1);
+
+    // Range 12g
+    busWrite(acc->busDev, REGA_RANGE, 0x02);
+    delay(1);
+
+    busSetSpeed(acc->busDev, BUS_SPEED_STANDARD);
+
+    acc->acc_1G = 2048;
+}
+
+static bool bmi088GyroRead(gyroDev_t *gyro)
+{
+    uint8_t gyroRaw[6];
+
+    if (busReadBuf(gyro->busDev, REGG_RATE_X_LSB, gyroRaw, 6)) {
+        gyro->gyroADCRaw[X] = (int16_t)((gyroRaw[1] << 8) | gyroRaw[0]);
+        gyro->gyroADCRaw[Y] = (int16_t)((gyroRaw[3] << 8) | gyroRaw[2]);
+        gyro->gyroADCRaw[Z] = (int16_t)((gyroRaw[5] << 8) | gyroRaw[4]);
+        return true;
+    }
+
+    return false;
+}
+
+static bool bmi088AccRead(accDev_t *acc)
+{
+    uint8_t buffer[7];
+
+    if (busReadBuf(acc->busDev, REGA_STATUS, buffer, 2) && (buffer[1] & 0x80) && busReadBuf(acc->busDev, REGA_X_LSB, buffer, 7)) {
+        // first byte is discarded, see datasheet
+        acc->ADCRaw[X] = (int32_t)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4);
+        acc->ADCRaw[Y] = (int32_t)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4);
+        acc->ADCRaw[Z] = (int32_t)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4);
+        return true;
+    }
+
+    return false;
+}
+
+static bool gyroDeviceDetect(busDevice_t * busDev)
+{
+    uint8_t attempts;
+
+    busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
+
+    for (attempts = 0; attempts < 5; attempts++) {
+        uint8_t chipId;
+
+        delay(100);
+        busRead(busDev, REGG_CHIPID, &chipId);
+
+        if (chipId == 0x0F) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+static bool accDeviceDetect(busDevice_t * busDev)
+{
+    uint8_t attempts;
+
+    busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
+
+    for (attempts = 0; attempts < 5; attempts++) {
+        uint8_t chipId;
+
+        delay(100);
+        busRead(busDev, REGA_CHIPID, &chipId);
+
+        if (chipId == 0x1E) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool bmi088AccDetect(accDev_t *acc)
+{
+    acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_ACC, acc->imuSensorToUse, OWNER_MPU);
+    if (acc->busDev == NULL) {
+        return false;
+    }
+
+    if (!accDeviceDetect(acc->busDev)) {
+        busDeviceDeInit(acc->busDev);
+        return false;
+    }
+
+    acc->initFn = bmi088AccInit;
+    acc->readFn = bmi088AccRead;
+
+    return true;
+}
+
+bool bmi088GyroDetect(gyroDev_t *gyro)
+{
+    gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_GYRO, gyro->imuSensorToUse, OWNER_MPU);
+    if (gyro->busDev == NULL) {
+        return false;
+    }
+
+    if (!gyroDeviceDetect(gyro->busDev)) {
+        busDeviceDeInit(gyro->busDev);
+        return false;
+    }
+
+    gyro->initFn = bmi088GyroInit;
+    gyro->readFn = bmi088GyroRead;
+    gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb
+    gyro->intStatusFn = gyroCheckDataReady;
+    gyro->gyroAlign = gyro->busDev->param;
+
+    return true;
+}
+
+#endif /* USE_IMU_BMI088 */
diff --git a/src/main/drivers/accgyro/accgyro_bmi088.h b/src/main/drivers/accgyro/accgyro_bmi088.h
new file mode 100644
index 00000000000..4ffa4acc3d1
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_bmi088.h
@@ -0,0 +1,30 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 "drivers/sensor.h"
+
+bool bmi088AccDetect(accDev_t *acc);
+bool bmi088GyroDetect(gyroDev_t *gyro);
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 6e7dc2f48b2..725de13fb2d 100755
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -91,6 +91,8 @@ typedef enum {
     DEVHW_MPU6050,
     DEVHW_MPU6500,
     DEVHW_BMI160,
+    DEVHW_BMI088_GYRO,
+    DEVHW_BMI088_ACC,
     DEVHW_ICM20689,
 
     /* Combined ACC/GYRO/MAG chips */
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 662ed10c2e8..68c14dc054f 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4,7 +4,7 @@ tables:
   - name: gyro_lpf
     values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"]
   - name: acc_hardware
-    values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"]
+    values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
     values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"]
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 9ff00c45297..241f1c417a0 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -50,6 +50,7 @@ FILE_COMPILE_FOR_SPEED
 #include "drivers/accgyro/accgyro_adxl345.h"
 #include "drivers/accgyro/accgyro_mma845x.h"
 #include "drivers/accgyro/accgyro_bma280.h"
+#include "drivers/accgyro/accgyro_bmi088.h"
 #include "drivers/accgyro/accgyro_bmi160.h"
 #include "drivers/accgyro/accgyro_icm20689.h"
 #include "drivers/accgyro/accgyro_fake.h"
@@ -240,6 +241,19 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
+#if defined(USE_IMU_BMI088)
+    case ACC_BMI088:
+        if (bmi088AccDetect(dev)) {
+            accHardware = ACC_BMI088;
+            break;
+        }
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (accHardwareToUse != ACC_AUTODETECT) {
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
 #ifdef USE_IMU_ICM20689
     case ACC_ICM20689:
         if (icm20689AccDetect(dev)) {
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index 47f669b83a5..a650b4be959 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -45,7 +45,8 @@ typedef enum {
     ACC_MPU9250 = 9,
     ACC_BMI160 = 10,
     ACC_ICM20689 = 11,
-    ACC_FAKE = 12,
+    ACC_BMI088 = 12,
+    ACC_FAKE = 13,
     ACC_MAX = ACC_FAKE
 } accelerationSensor_e;
 
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index d9cd15891d9..1ce3c169ba8 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -52,6 +52,7 @@ FILE_COMPILE_FOR_SPEED
 #include "drivers/accgyro/accgyro_adxl345.h"
 #include "drivers/accgyro/accgyro_mma845x.h"
 #include "drivers/accgyro/accgyro_bma280.h"
+#include "drivers/accgyro/accgyro_bmi088.h"
 #include "drivers/accgyro/accgyro_bmi160.h"
 #include "drivers/accgyro/accgyro_icm20689.h"
 #include "drivers/accgyro/accgyro_fake.h"
@@ -202,6 +203,15 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
+#ifdef USE_IMU_BMI088
+    case GYRO_BMI088:
+        if (bmi088GyroDetect(dev)) {
+            gyroHardware = GYRO_BMI088;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
 #ifdef USE_IMU_ICM20689
     case GYRO_ICM20689:
         if (icm20689GyroDetect(dev)) {
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index b709294b553..d47a02f6f68 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -36,6 +36,7 @@ typedef enum {
     GYRO_MPU9250,
     GYRO_BMI160,
     GYRO_ICM20689,
+    GYRO_BMI088,
     GYRO_FAKE
 } gyroSensor_e;
 
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index ad8736d0db6..b445a131936 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -80,6 +80,15 @@
         BUSDEV_REGISTER_I2C(busdev_bmi160,      DEVHW_BMI160,       BMI160_I2C_BUS,     0x68,               GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI160_ALIGN);
         #endif
     #endif
+
+    #if defined(USE_IMU_BMI088)
+        #if defined(BMI088_SPI_BUS)
+        BUSDEV_REGISTER_SPI(busdev_bmi088_gyro, DEVHW_BMI088_GYRO,  BMI088_SPI_BUS,     BMI088_GYRO_CS_PIN, GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI088_ALIGN);
+        BUSDEV_REGISTER_SPI(busdev_bmi088_acc,  DEVHW_BMI088_ACC,   BMI088_SPI_BUS,     BMI088_ACC_CS_PIN,  GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI088_ALIGN);
+        #elif defined(BMI088_I2C_BUS)
+        BUSDEV_REGISTER_I2C(busdev_bmi088,      DEVHW_BMI088,       BMI088_I2C_BUS,     0x68,               GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI088_ALIGN);
+        #endif
+    #endif
 #endif
 
 

From 40ed910bfe3c1c378de03a859414449c066ff751 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Mon, 7 Dec 2020 13:06:34 +0100
Subject: [PATCH 02/50] First try

---
 src/main/flight/pid.c | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 00cd8ac9c5c..b4755491718 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -611,7 +611,15 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float newFFTerm = pidState->rateTarget * pidState->kFF;
 
     // Calculate integral
-    pidState->errorGyroIf += rateError * pidState->kI * dT;
+    // If bank angle is more than 20 degrees do not update yaw and pitch I-term
+    float bankAngle = attitude.values.roll;
+    if (bankAngle > 100 && axis == FD_YAW) {
+        pidState->errorGyroIf += 0;
+    } else
+    {
+        pidState->errorGyroIf += rateError * pidState->kI * dT;
+    }
+    
 
     applyItermLimiting(pidState);
 

From dc0b9be9459d583793ccfd01e90ba24c6b870bd7 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Mon, 7 Dec 2020 14:13:06 +0100
Subject: [PATCH 03/50] Create CLI command.

---
 src/main/fc/settings.yaml |  6 ++++++
 src/main/flight/pid.c     | 10 +++++-----
 src/main/flight/pid.h     |  1 +
 3 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 4316e29f7ad..a66b07bab3c 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1569,6 +1569,12 @@ groups:
         field: fixedWingItermLimitOnStickPosition
         min: 0
         max: 1
+      - name: fw_iterm_limit_bank_angle
+        description: "Iterm is not allowed to grow when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 180 (the default) disables this feature."
+        default_value: "180"
+        field: fixedWingItermBankLimit
+        min: 0
+        max: 180
       - name: pidsum_limit
         description: "A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help"
         default_value: "500"
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index b4755491718..8d9f4b17c99 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -258,6 +258,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingCoordinatedYawGain = 1.0f,
         .fixedWingCoordinatedPitchGain = 1.0f,
         .fixedWingItermLimitOnStickPosition = 0.5f,
+        .fixedWingItermBankLimit = 180,
 
         .loiter_direction = NAV_LOITER_RIGHT,
         .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ,
@@ -611,15 +612,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float newFFTerm = pidState->rateTarget * pidState->kFF;
 
     // Calculate integral
-    // If bank angle is more than 20 degrees do not update yaw and pitch I-term
-    float bankAngle = attitude.values.roll;
-    if (bankAngle > 100 && axis == FD_YAW) {
+    // Freeze yaw Iterm when bank angle is above threshold to avoid rudder counteracting turns
+    float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
+    if (fabsf(bankAngle) > pidProfile()->fixedWingItermBankLimit && axis == FD_YAW) {
         pidState->errorGyroIf += 0;
     } else
     {
         pidState->errorGyroIf += rateError * pidState->kI * dT;
     }
-    
 
     applyItermLimiting(pidState);
 
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index cc4b20ae599..aa7670f5ebf 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -128,6 +128,7 @@ typedef struct pidProfile_s {
     float       fixedWingCoordinatedYawGain;    // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
     float       fixedWingCoordinatedPitchGain;    // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
     float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
+    uint16_t    fixedWingItermBankLimit;       // Freeze yaw Iterm when bank angle is more than this many degrees
 
     uint8_t     loiter_direction;               // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
     float       navVelXyDTermLpfHz;

From 36124be148132c15204086d2e24634e14ada0a33 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Mon, 7 Dec 2020 14:17:12 +0100
Subject: [PATCH 04/50] Rename variables to make more clear that this only
 affects yaw stabilization.

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

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a66b07bab3c..afa2506a01c 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1569,10 +1569,10 @@ groups:
         field: fixedWingItermLimitOnStickPosition
         min: 0
         max: 1
-      - name: fw_iterm_limit_bank_angle
-        description: "Iterm is not allowed to grow when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 180 (the default) disables this feature."
+      - name: fw_yaw_iterm_limit_bank_angle
+        description: "Yaw Iterm is not allowed to grow when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 180 (the default) disables this feature."
         default_value: "180"
-        field: fixedWingItermBankLimit
+        field: fixedWingYawItermBankLimit
         min: 0
         max: 180
       - name: pidsum_limit
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 8d9f4b17c99..d83e3cd09e5 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -258,7 +258,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingCoordinatedYawGain = 1.0f,
         .fixedWingCoordinatedPitchGain = 1.0f,
         .fixedWingItermLimitOnStickPosition = 0.5f,
-        .fixedWingItermBankLimit = 180,
+        .fixedWingYawItermBankLimit = 180,
 
         .loiter_direction = NAV_LOITER_RIGHT,
         .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ,
@@ -614,7 +614,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     // Calculate integral
     // Freeze yaw Iterm when bank angle is above threshold to avoid rudder counteracting turns
     float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
-    if (fabsf(bankAngle) > pidProfile()->fixedWingItermBankLimit && axis == FD_YAW) {
+    if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && axis == FD_YAW) {
         pidState->errorGyroIf += 0;
     } else
     {
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index aa7670f5ebf..9f8090ef6c4 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -128,7 +128,7 @@ typedef struct pidProfile_s {
     float       fixedWingCoordinatedYawGain;    // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
     float       fixedWingCoordinatedPitchGain;    // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
     float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
-    uint16_t    fixedWingItermBankLimit;       // Freeze yaw Iterm when bank angle is more than this many degrees
+    uint16_t    fixedWingYawItermBankLimit;       // Freeze yaw Iterm when bank angle is more than this many degrees
 
     uint8_t     loiter_direction;               // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
     float       navVelXyDTermLpfHz;

From 3c8deadb3e2b95ffa6cf7cfc3c222877f31e2dd9 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Mon, 7 Dec 2020 15:41:13 +0100
Subject: [PATCH 05/50] Don't freeze I-term when in AUTO TUNE (not sure if
 necessary, just to be safe).

---
 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 d83e3cd09e5..cc0bbc5f954 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -614,7 +614,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     // Calculate integral
     // Freeze yaw Iterm when bank angle is above threshold to avoid rudder counteracting turns
     float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
-    if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && axis == FD_YAW) {
+    if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && axis == FD_YAW && !FLIGHT_MODE(AUTO_TUNE)) {
         pidState->errorGyroIf += 0;
     } else
     {

From 51b2a49cb4a1aa2bf04595137ea65a59995f50aa Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 9 Dec 2020 09:55:28 +0100
Subject: [PATCH 06/50] Implement suggestions. Stops I-term from growing
 instead of freezing it.

---
 src/main/flight/pid.c | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index cc0bbc5f954..ae6910dcf44 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -612,14 +612,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float newFFTerm = pidState->rateTarget * pidState->kFF;
 
     // Calculate integral
-    // Freeze yaw Iterm when bank angle is above threshold to avoid rudder counteracting turns
-    float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
-    if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && axis == FD_YAW && !FLIGHT_MODE(AUTO_TUNE)) {
-        pidState->errorGyroIf += 0;
-    } else
-    {
-        pidState->errorGyroIf += rateError * pidState->kI * dT;
-    }
+    pidState->errorGyroIf += rateError * pidState->kI * dT;
 
     applyItermLimiting(pidState);
 
@@ -928,11 +921,18 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
 }
 
-void checkItermLimitingActive(pidState_t *pidState)
+void checkItermLimitingActive(pidState_t *pidState, flight_dynamics_index_t axis)
 {
     bool shouldActivate;
+    float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
     if (usedPidControllerType == PID_TYPE_PIFF) {
-        shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
+        // Do not allow yaw I-term to grow when bank angle is too large
+        if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit){
+            shouldActivate = 1;
+        } else
+        {
+            shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
+        }
     } else 
     {
         shouldActivate = mixerIsOutputSaturated();
@@ -1010,7 +1010,7 @@ void FAST_CODE pidController(float dT)
         pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
         
         // Step 4: Run gyro-driven control
-        checkItermLimitingActive(&pidState[axis]);
+        checkItermLimitingActive(&pidState[axis], axis);
         pidControllerApplyFn(&pidState[axis], axis, dT);
     }
 }

From aac48f4868eb454aff2c31046d2d46b737a25526 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 9 Dec 2020 09:56:39 +0100
Subject: [PATCH 07/50] Again forgot  AUTO TUNE

---
 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 ae6910dcf44..f908a03fe24 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -927,7 +927,7 @@ void checkItermLimitingActive(pidState_t *pidState, flight_dynamics_index_t axis
     float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
     if (usedPidControllerType == PID_TYPE_PIFF) {
         // Do not allow yaw I-term to grow when bank angle is too large
-        if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit){
+        if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !FLIGHT_MODE(AUTO_TUNE)){
             shouldActivate = 1;
         } else
         {

From 53f9c9f5ee2900fb48cb1cade1b0ef010f597ab8 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 9 Dec 2020 11:26:45 +0100
Subject: [PATCH 08/50] Freezing I-term instead, using new functions.

---
 src/main/flight/pid.c | 45 ++++++++++++++++++++++++++++++++-----------
 1 file changed, 34 insertions(+), 11 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index f908a03fe24..5547273abe4 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -81,6 +81,7 @@ typedef struct {
     // Rate integrator
     float errorGyroIf;
     float errorGyroIfLimit;
+    float errorGyroIfFrozen;
 
     // Used for ANGLE filtering (PT1, we don't need super-sharpness here)
     pt1Filter_t angleFilterState;
@@ -103,6 +104,7 @@ typedef struct {
     uint16_t pidSumLimit;
     filterApply4FnPtr ptermFilterApplyFn;
     bool itermLimitActive;
+    bool itermFreezeActive;
 
     biquadFilter_t rateTargetFilter;
 } pidState_t;
@@ -351,6 +353,7 @@ void pidResetErrorAccumulators(void)
     for (int axis = 0; axis < 3; axis++) {
         pidState[axis].errorGyroIf = 0.0f;
         pidState[axis].errorGyroIfLimit = 0.0f;
+        pidState[axis].errorGyroIfFrozen = 0.0f;
     }
 }
 
@@ -594,11 +597,21 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
 static void applyItermLimiting(pidState_t *pidState) {
     if (pidState->itermLimitActive) {
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
-    } else {
+    } else 
+    {
         pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
     }
 }
 
+static void applyItermFreezing(pidState_t *pidState) {
+    if (pidState->itermFreezeActive) {
+        pidState->errorGyroIf = pidState->errorGyroIfFrozen;
+    } else 
+    {
+        pidState->errorGyroIfFrozen = pidState->errorGyroIf;
+    }
+}
+
 static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
     UNUSED(pidState);
     UNUSED(axis);
@@ -615,6 +628,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     pidState->errorGyroIf += rateError * pidState->kI * dT;
 
     applyItermLimiting(pidState);
+    applyItermFreezing(pidState);
 
     if (pidProfile()->fixedWingItermThrowLimit != 0) {
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
@@ -921,18 +935,11 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
 }
 
-void checkItermLimitingActive(pidState_t *pidState, flight_dynamics_index_t axis)
+void checkItermLimitingActive(pidState_t *pidState)
 {
     bool shouldActivate;
-    float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
     if (usedPidControllerType == PID_TYPE_PIFF) {
-        // Do not allow yaw I-term to grow when bank angle is too large
-        if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !FLIGHT_MODE(AUTO_TUNE)){
-            shouldActivate = 1;
-        } else
-        {
-            shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
-        }
+        shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
     } else 
     {
         shouldActivate = mixerIsOutputSaturated();
@@ -941,6 +948,20 @@ void checkItermLimitingActive(pidState_t *pidState, flight_dynamics_index_t axis
     pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; 
 }
 
+void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
+{
+    if (usedPidControllerType == PID_TYPE_PIFF) {
+        // Do not allow yaw I-term to grow when bank angle is too large
+        float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
+        if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !FLIGHT_MODE(AUTO_TUNE)){
+            pidState->itermFreezeActive = true;
+        } else
+        {
+            pidState->itermFreezeActive = false;
+        }
+    }
+}
+
 void FAST_CODE pidController(float dT)
 {
     if (!pidFiltersConfigured) {
@@ -1010,7 +1031,9 @@ void FAST_CODE pidController(float dT)
         pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
         
         // Step 4: Run gyro-driven control
-        checkItermLimitingActive(&pidState[axis], axis);
+        checkItermLimitingActive(&pidState[axis]);
+        checkItermFreezingActive(&pidState[axis], axis);
+
         pidControllerApplyFn(&pidState[axis], axis, dT);
     }
 }

From 4b9d388a72a27afe59593cbe09cdb50fcfdebab7 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 9 Dec 2020 17:39:07 +0100
Subject: [PATCH 09/50] Disable in all cases where TURN ASSIST is on

---
 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 5547273abe4..2bb4b1e9a3d 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -953,7 +953,7 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
     if (usedPidControllerType == PID_TYPE_PIFF) {
         // Do not allow yaw I-term to grow when bank angle is too large
         float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
-        if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !FLIGHT_MODE(AUTO_TUNE)){
+        if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
             pidState->itermFreezeActive = true;
         } else
         {

From 7c5b15e6215420ebd0f730debb6cab2784100692 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 9 Dec 2020 23:09:24 +0100
Subject: [PATCH 10/50] Changed settings and fixed possible issue on
 multirotors.

---
 src/main/fc/settings.yaml | 6 +++---
 src/main/flight/pid.c     | 6 +++++-
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index afa2506a01c..3557cdd886e 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1570,11 +1570,11 @@ groups:
         min: 0
         max: 1
       - name: fw_yaw_iterm_limit_bank_angle
-        description: "Yaw Iterm is not allowed to grow when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 180 (the default) disables this feature."
-        default_value: "180"
+        description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled."
+        default_value: "0"
         field: fixedWingYawItermBankLimit
         min: 0
-        max: 180
+        max: 90
       - name: pidsum_limit
         description: "A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help"
         default_value: "500"
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 2bb4b1e9a3d..e69c99b3cb0 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -950,7 +950,7 @@ void checkItermLimitingActive(pidState_t *pidState)
 
 void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
 {
-    if (usedPidControllerType == PID_TYPE_PIFF) {
+    if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankLimit != 0) {
         // Do not allow yaw I-term to grow when bank angle is too large
         float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
         if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
@@ -959,7 +959,11 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
         {
             pidState->itermFreezeActive = false;
         }
+    } else
+    {
+        pidState->itermFreezeActive = false;
     }
+    
 }
 
 void FAST_CODE pidController(float dT)

From 8e45887bc41396837792feb08ee09972648cfa55 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Thu, 10 Dec 2020 09:56:23 +0100
Subject: [PATCH 11/50] Move yaw axis check to outer if-statement

---
 src/main/flight/pid.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index e69c99b3cb0..058988ea763 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -950,10 +950,10 @@ void checkItermLimitingActive(pidState_t *pidState)
 
 void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
 {
-    if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankLimit != 0) {
+    if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankLimit != 0 && axis == FD_YAW) {
         // Do not allow yaw I-term to grow when bank angle is too large
         float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
-        if (axis == FD_YAW && fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
+        if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
             pidState->itermFreezeActive = true;
         } else
         {

From ca9600a8a12cdd153603b159532560c919c2ff22 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Thu, 10 Dec 2020 10:11:01 +0100
Subject: [PATCH 12/50] Fix default value issue

---
 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 058988ea763..21190bb74c0 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -260,7 +260,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingCoordinatedYawGain = 1.0f,
         .fixedWingCoordinatedPitchGain = 1.0f,
         .fixedWingItermLimitOnStickPosition = 0.5f,
-        .fixedWingYawItermBankLimit = 180,
+        .fixedWingYawItermBankLimit = 0,
 
         .loiter_direction = NAV_LOITER_RIGHT,
         .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ,

From 51c0b95cbff5201480ffa1b0f1f9815ba99424d6 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sat, 12 Dec 2020 10:40:29 +0100
Subject: [PATCH 13/50] Make TURN ASSIST axis aware, use target bank angle, and
 disable in ACRO mode

---
 src/main/flight/pid.c | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 00cd8ac9c5c..e9a3c3859d4 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -843,13 +843,13 @@ float pidHeadingHold(float dT)
  * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
  * and keeping ROLL and PITCH attitude though the turn.
  */
-static void NOINLINE pidTurnAssistant(pidState_t *pidState)
+static void NOINLINE pidTurnAssistant(pidState_t *pidState, flight_dynamics_index_t axis)
 {
     fpVector3_t targetRates;
     targetRates.x = 0.0f;
     targetRates.y = 0.0f;
 
-    if (STATE(AIRPLANE)) {
+    if (STATE(AIRPLANE) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
         if (calculateCosTiltAngle() >= 0.173648f) {
             // Ideal banked turn follow the equations:
             //      forward_vel^2 / radius = Gravity * tan(roll_angle)
@@ -871,8 +871,8 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
 
             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
-            float bankAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
-            float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn;
+            float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[axis]);
+            float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn;
 
             targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
         }
@@ -986,7 +986,9 @@ void FAST_CODE pidController(float dT)
     }
 
     if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
-        pidTurnAssistant(pidState);
+        for (int axis = 0; axis < 3; axis++) {
+            pidTurnAssistant(pidState, axis);
+        }
         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
     }
 

From 7ec6afd1f3f9d4a385af8126252093f4ef7c5f1e Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sat, 12 Dec 2020 10:54:01 +0100
Subject: [PATCH 14/50] Change target bank angle code

---
 src/main/flight/pid.c | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index e9a3c3859d4..40eebf7a8e1 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -843,7 +843,7 @@ float pidHeadingHold(float dT)
  * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
  * and keeping ROLL and PITCH attitude though the turn.
  */
-static void NOINLINE pidTurnAssistant(pidState_t *pidState, flight_dynamics_index_t axis)
+static void NOINLINE pidTurnAssistant(pidState_t *pidState)
 {
     fpVector3_t targetRates;
     targetRates.x = 0.0f;
@@ -871,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, flight_dynamics_inde
             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
 
             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
-            float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[axis]);
+            float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[ROLL]);
             float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn;
 
             targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
@@ -986,9 +986,9 @@ void FAST_CODE pidController(float dT)
     }
 
     if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
-        for (int axis = 0; axis < 3; axis++) {
-            pidTurnAssistant(pidState, axis);
-        }
+        //for (int axis = 0; axis < 3; axis++) {
+            pidTurnAssistant(pidState);
+        //}
         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
     }
 

From 232d526ab12549e8d1553505aba8af3c5cd4c952 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sat, 12 Dec 2020 11:31:46 +0100
Subject: [PATCH 15/50] no message

---
 src/main/flight/pid.c | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 40eebf7a8e1..f3909660230 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -871,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
 
             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
-            float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[ROLL]);
+            float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
             float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn;
 
             targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
@@ -986,9 +986,7 @@ void FAST_CODE pidController(float dT)
     }
 
     if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
-        //for (int axis = 0; axis < 3; axis++) {
-            pidTurnAssistant(pidState);
-        //}
+        pidTurnAssistant(pidState);
         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
     }
 

From 214f939a1f06e51cb402fde9eacfaccf81da1a9a Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sat, 12 Dec 2020 14:21:45 +0100
Subject: [PATCH 16/50] Attempted fix (does not work)

---
 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 f3909660230..a21ae688d36 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -871,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
 
             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
-            float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
+            float bankAngleTarget = pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
             float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn;
 
             targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);

From cf2e35908adbfbe4a6bbc726c896c86f68c1f83b Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sat, 12 Dec 2020 21:42:19 +0100
Subject: [PATCH 17/50] Convert bank angle to radians

---
 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 a21ae688d36..0623c826b60 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -871,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
 
             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
-            float bankAngleTarget = pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
+            float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
             float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn;
 
             targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);

From 95ab3fe22b1ad336e6898e2d9a5bc2339d467acf Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sun, 13 Dec 2020 11:02:42 +0100
Subject: [PATCH 18/50] Take target bank angle calculation out of
 pidTurnAssistant

---
 src/main/flight/pid.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 0623c826b60..000e33c9faa 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -843,7 +843,7 @@ float pidHeadingHold(float dT)
  * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
  * and keeping ROLL and PITCH attitude though the turn.
  */
-static void NOINLINE pidTurnAssistant(pidState_t *pidState)
+static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget)
 {
     fpVector3_t targetRates;
     targetRates.x = 0.0f;
@@ -871,7 +871,6 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
 
             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
-            float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
             float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn;
 
             targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
@@ -986,7 +985,8 @@ void FAST_CODE pidController(float dT)
     }
 
     if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
-        pidTurnAssistant(pidState);
+        float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
+        pidTurnAssistant(pidState, bankAngleTarget);
         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
     }
 

From 2270a8c3df9a926e74378de05b962cdbc7283537 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sun, 13 Dec 2020 11:15:09 +0100
Subject: [PATCH 19/50] Modes check outside of pidTurnAssistant

---
 src/main/flight/pid.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 000e33c9faa..8d94c3b4fcd 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -849,7 +849,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge
     targetRates.x = 0.0f;
     targetRates.y = 0.0f;
 
-    if (STATE(AIRPLANE) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
+    if (STATE(AIRPLANE)) {
         if (calculateCosTiltAngle() >= 0.173648f) {
             // Ideal banked turn follow the equations:
             //      forward_vel^2 / radius = Gravity * tan(roll_angle)
@@ -984,7 +984,7 @@ void FAST_CODE pidController(float dT)
         levelingEnabled = false;
     }
 
-    if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
+    if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
         float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
         pidTurnAssistant(pidState, bankAngleTarget);
         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT

From a1f42fcfe2cc287a8ad37a04fc89d5542fadb0e5 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Mon, 14 Dec 2020 15:24:50 +0100
Subject: [PATCH 20/50] Take pitch angle into account for turn rate calculation
 (technically correct but doesn't make huge difference)

---
 src/main/flight/pid.c | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 8d94c3b4fcd..e733bc43bfc 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -843,7 +843,7 @@ float pidHeadingHold(float dT)
  * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
  * and keeping ROLL and PITCH attitude though the turn.
  */
-static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget)
+static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget, float pitchAngleTarget)
 {
     fpVector3_t targetRates;
     targetRates.x = 0.0f;
@@ -871,7 +871,8 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge
             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
 
             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
-            float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn;
+            float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget));
+            float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor;
 
             targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
         }
@@ -986,7 +987,8 @@ void FAST_CODE pidController(float dT)
 
     if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
         float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
-        pidTurnAssistant(pidState, bankAngleTarget);
+        float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
+        pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
     }
 

From 37f2954ad1c33902e6c4a9362d5538e4646d038a Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Tue, 15 Dec 2020 14:41:01 +0100
Subject: [PATCH 21/50] Constain bank angle for calculation to 60 degrees.

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

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index e733bc43bfc..1f27af2b6f5 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -871,6 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge
             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
 
             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
+            bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60));
             float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget));
             float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor;
 

From 1ff79a9f155ad4fdc378c93a842e492af16f9856 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sun, 3 Jan 2021 14:35:51 +0100
Subject: [PATCH 22/50] Change CLI and variable names to align with
 functionality

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

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 3557cdd886e..baf7e48a15c 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1569,10 +1569,10 @@ groups:
         field: fixedWingItermLimitOnStickPosition
         min: 0
         max: 1
-      - name: fw_yaw_iterm_limit_bank_angle
+      - name: fw_yaw_iterm_freeze_bank_angle
         description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled."
         default_value: "0"
-        field: fixedWingYawItermBankLimit
+        field: fixedWingYawItermBankFreeze
         min: 0
         max: 90
       - name: pidsum_limit
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 21190bb74c0..493c2192f7c 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -260,7 +260,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingCoordinatedYawGain = 1.0f,
         .fixedWingCoordinatedPitchGain = 1.0f,
         .fixedWingItermLimitOnStickPosition = 0.5f,
-        .fixedWingYawItermBankLimit = 0,
+        .fixedWingYawItermBankFreeze = 0,
 
         .loiter_direction = NAV_LOITER_RIGHT,
         .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ,
@@ -950,10 +950,10 @@ void checkItermLimitingActive(pidState_t *pidState)
 
 void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
 {
-    if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankLimit != 0 && axis == FD_YAW) {
+    if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) {
         // Do not allow yaw I-term to grow when bank angle is too large
         float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
-        if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankLimit && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
+        if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
             pidState->itermFreezeActive = true;
         } else
         {
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 9f8090ef6c4..7a9560c86ae 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -128,7 +128,7 @@ typedef struct pidProfile_s {
     float       fixedWingCoordinatedYawGain;    // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
     float       fixedWingCoordinatedPitchGain;    // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
     float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
-    uint16_t    fixedWingYawItermBankLimit;       // Freeze yaw Iterm when bank angle is more than this many degrees
+    uint16_t    fixedWingYawItermBankFreeze;       // Freeze yaw Iterm when bank angle is more than this many degrees
 
     uint8_t     loiter_direction;               // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
     float       navVelXyDTermLpfHz;

From 5613059137e106f57c2fa180538e379b211cca05 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sun, 3 Jan 2021 16:11:07 +0100
Subject: [PATCH 23/50] Update docs

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 7c53b289d4b..e5481fbf84b 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -130,6 +130,7 @@
 | fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups |
 | fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter |
 | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter |
+| fw_yaw_iterm_freeze_bank_angle | 0 | Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. |
 | gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports |
 | gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. |
 | gps_dyn_model | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. |

From 3305d536bcae745d4594028743f4d39b98710efe Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 13 Jan 2021 13:53:05 +0100
Subject: [PATCH 24/50] pan servo home dir offset

---
 src/main/fc/settings.yaml | 14 ++++++++++++++
 src/main/io/osd.c         | 19 +++++++++++++++++--
 src/main/io/osd.h         |  3 ++-
 3 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index f5fde879efe..f520de2d177 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3015,6 +3015,20 @@ groups:
         default_value: "ON"
         description: Should home position coordinates be displayed on the arming screen.
 
+      - name: osd_pan_servo_index
+        type: uint8_t
+        field: pan_servo_index
+        min: 0
+        max: 10
+
+      - name: osd_pan_servo_us2centideg
+        type: uint16_t
+        field: pan_servo_us2centideg
+        default_value: 9
+        min: -36
+        max: 36
+
+
   - name: PG_SYSTEM_CONFIG
     type: systemConfig_t
     headers: ["fc/config.h"]
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index ae077441bed..b9f1f507bea 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -87,6 +87,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/pid.h"
 #include "flight/rth_estimator.h"
 #include "flight/wind_estimator.h"
+#include "flight/servos.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
@@ -185,7 +186,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
 
 static int digitCount(int32_t value)
@@ -1128,6 +1129,14 @@ static int16_t osdGet3DSpeed(void)
     return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
 }
 
+static int16_t osdPanServoHomeDirectionOffset(void)
+{
+    int8_t servoIndex = osdConfig()->pan_servo_index;
+    int16_t servoPosition = servo[servoIndex];
+    int16_t servoMiddle = servoParams(servoIndex)->middle;
+    return (int16_t)((servoPosition - servoMiddle) * CENTIDEGREES_TO_DEGREES(osdConfig()->pan_servo_us2centideg));
+}
+
 #endif
 
 static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) {
@@ -1337,7 +1346,11 @@ static bool osdDrawSingleElement(uint8_t item)
                 }
                 else
                 {
-                    int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading());
+                    int16_t panHomeDirOffset = 0;
+                    if (!osdConfig()->pan_servo_us2centideg){
+                        panHomeDirOffset = osdPanServoHomeDirectionOffset();
+                    }
+                    int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset;
                     osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection);
                 }
             } else {
@@ -2591,6 +2604,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE,
     .sidebar_scroll_arrows = 0,
     .osd_home_position_arm_screen = true,
+    .pan_servo_index = 0,
+    .pan_servo_us2centideg = 0,
 
     .units = OSD_UNIT_METRIC,
     .main_voltage_decimals = 1,
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index d740b5de521..6dce63dadff 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -348,7 +348,8 @@ typedef struct osdConfig_s {
     uint8_t left_sidebar_scroll_step;   // How many units each sidebar step represents. 0 means the default value for the scroll type.
     uint8_t right_sidebar_scroll_step;  // Same as left_sidebar_scroll_step, but for the right sidebar.
     bool osd_home_position_arm_screen;
-
+    uint8_t pan_servo_index;            // Index of the pan servo used for home direction offset
+    uint16_t pan_servo_us2centideg;          // cenitdegrees per us pwm
     uint8_t crsf_lq_format;
 
 } osdConfig_t;

From 4eaa18b928edabdcf3a2372171caed566945c0da Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 13 Jan 2021 14:28:27 +0100
Subject: [PATCH 25/50] changed variable type

---
 src/main/fc/settings.yaml | 2 +-
 src/main/io/osd.c         | 4 ++--
 src/main/io/osd.h         | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index f520de2d177..44d739e5363 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3022,7 +3022,7 @@ groups:
         max: 10
 
       - name: osd_pan_servo_us2centideg
-        type: uint16_t
+        type: float
         field: pan_servo_us2centideg
         default_value: 9
         min: -36
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index b9f1f507bea..50cf3947550 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1134,7 +1134,7 @@ static int16_t osdPanServoHomeDirectionOffset(void)
     int8_t servoIndex = osdConfig()->pan_servo_index;
     int16_t servoPosition = servo[servoIndex];
     int16_t servoMiddle = servoParams(servoIndex)->middle;
-    return (int16_t)((servoPosition - servoMiddle) * CENTIDEGREES_TO_DEGREES(osdConfig()->pan_servo_us2centideg));
+    return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_us2centideg);
 }
 
 #endif
@@ -1347,7 +1347,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 else
                 {
                     int16_t panHomeDirOffset = 0;
-                    if (!osdConfig()->pan_servo_us2centideg){
+                    if (!(osdConfig()->pan_servo_us2centideg == 0)){
                         panHomeDirOffset = osdPanServoHomeDirectionOffset();
                     }
                     int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 6dce63dadff..07a861e9c9e 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -349,7 +349,7 @@ typedef struct osdConfig_s {
     uint8_t right_sidebar_scroll_step;  // Same as left_sidebar_scroll_step, but for the right sidebar.
     bool osd_home_position_arm_screen;
     uint8_t pan_servo_index;            // Index of the pan servo used for home direction offset
-    uint16_t pan_servo_us2centideg;          // cenitdegrees per us pwm
+    float pan_servo_us2centideg;          // cenitdegrees per us pwm
     uint8_t crsf_lq_format;
 
 } osdConfig_t;

From 74dbdc9bb0a6c066a1630ccfdd1dcf05c98b5ffd Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 13 Jan 2021 14:29:25 +0100
Subject: [PATCH 26/50] For bench testing

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 50cf3947550..14841212a52 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1340,7 +1340,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_HOME_DIR:
         {
-            if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
+            if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { // && isImuHeadingValid()
                 if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
                     displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
                 }

From 0c42ae1f62f7f9c3e032fec8be27e147b8a62a18 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 13 Jan 2021 15:00:09 +0100
Subject: [PATCH 27/50] Some improvements.

---
 src/main/fc/settings.yaml |  6 +++---
 src/main/io/osd.c         | 18 +++++++++---------
 src/main/io/osd.h         |  2 +-
 3 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 44d739e5363..ef3dc38c128 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3016,15 +3016,15 @@ groups:
         description: Should home position coordinates be displayed on the arming screen.
 
       - name: osd_pan_servo_index
-        type: uint8_t
+        description: Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos.
         field: pan_servo_index
         min: 0
         max: 10
 
       - name: osd_pan_servo_us2centideg
-        type: float
+        description: Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting.
         field: pan_servo_us2centideg
-        default_value: 9
+        default_value: 0
         min: -36
         max: 36
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 14841212a52..5f97bea74e2 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -949,6 +949,14 @@ int16_t osdGetHeading(void)
     return attitude.values.yaw;
 }
 
+int16_t osdPanServoHomeDirectionOffset(void)
+{
+    int8_t servoIndex = osdConfig()->pan_servo_index;
+    int16_t servoPosition = servo[servoIndex];
+    int16_t servoMiddle = servoParams(servoIndex)->middle;
+    return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_us2centideg);
+}
+
 // Returns a heading angle in degrees normalized to [0, 360).
 int osdGetHeadingAngle(int angle)
 {
@@ -1129,14 +1137,6 @@ static int16_t osdGet3DSpeed(void)
     return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
 }
 
-static int16_t osdPanServoHomeDirectionOffset(void)
-{
-    int8_t servoIndex = osdConfig()->pan_servo_index;
-    int16_t servoPosition = servo[servoIndex];
-    int16_t servoMiddle = servoParams(servoIndex)->middle;
-    return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_us2centideg);
-}
-
 #endif
 
 static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) {
@@ -1340,7 +1340,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_HOME_DIR:
         {
-            if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { // && isImuHeadingValid()
+            if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
                 if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
                     displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
                 }
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 07a861e9c9e..021113a0090 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -349,7 +349,7 @@ typedef struct osdConfig_s {
     uint8_t right_sidebar_scroll_step;  // Same as left_sidebar_scroll_step, but for the right sidebar.
     bool osd_home_position_arm_screen;
     uint8_t pan_servo_index;            // Index of the pan servo used for home direction offset
-    float pan_servo_us2centideg;          // cenitdegrees per us pwm
+    int8_t pan_servo_us2centideg;          // centidegrees per us pwm
     uint8_t crsf_lq_format;
 
 } osdConfig_t;

From 40cbcc7e45dd910a460901c34e5e9ca448d06e7a Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 13 Jan 2021 15:03:29 +0100
Subject: [PATCH 28/50] Update docs

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 547e03cacde..0bd5cf8c28a 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -389,6 +389,8 @@
 | osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% |
 | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. |
 | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) |
+| osd_pan_servo_index |  | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. |
+| osd_pan_servo_us2centideg | 0 | Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. |
 | osd_plus_code_digits | 10 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. |
 | osd_plus_code_short | 0 | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. |
 | osd_right_sidebar_scroll |  |  |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index ef3dc38c128..b5e9ada3466 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3022,7 +3022,7 @@ groups:
         max: 10
 
       - name: osd_pan_servo_us2centideg
-        description: Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting.
+        description: Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction.
         field: pan_servo_us2centideg
         default_value: 0
         min: -36

From 3117a2ee59811efb638b59322a0ad8dffede26fb Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Wed, 13 Jan 2021 15:09:05 +0100
Subject: [PATCH 29/50] change command name

---
 docs/Settings.md          | 2 +-
 src/main/fc/settings.yaml | 6 +++---
 src/main/io/osd.c         | 6 +++---
 src/main/io/osd.h         | 2 +-
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 0bd5cf8c28a..e2302536101 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -390,7 +390,7 @@
 | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. |
 | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) |
 | osd_pan_servo_index |  | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. |
-| osd_pan_servo_us2centideg | 0 | Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. |
+| osd_pan_servo_pwm2centideg | 0 | Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. |
 | osd_plus_code_digits | 10 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. |
 | osd_plus_code_short | 0 | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. |
 | osd_right_sidebar_scroll |  |  |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index b5e9ada3466..78ad14710e9 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3021,9 +3021,9 @@ groups:
         min: 0
         max: 10
 
-      - name: osd_pan_servo_us2centideg
-        description: Centidegrees of rotation of the pan servo per us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction.
-        field: pan_servo_us2centideg
+      - name: osd_pan_servo_pwm2centideg
+        description: Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction.
+        field: pan_servo_pwm2centideg
         default_value: 0
         min: -36
         max: 36
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 5f97bea74e2..660db14c038 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -954,7 +954,7 @@ int16_t osdPanServoHomeDirectionOffset(void)
     int8_t servoIndex = osdConfig()->pan_servo_index;
     int16_t servoPosition = servo[servoIndex];
     int16_t servoMiddle = servoParams(servoIndex)->middle;
-    return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_us2centideg);
+    return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg);
 }
 
 // Returns a heading angle in degrees normalized to [0, 360).
@@ -1347,7 +1347,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 else
                 {
                     int16_t panHomeDirOffset = 0;
-                    if (!(osdConfig()->pan_servo_us2centideg == 0)){
+                    if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
                         panHomeDirOffset = osdPanServoHomeDirectionOffset();
                     }
                     int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset;
@@ -2605,7 +2605,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .sidebar_scroll_arrows = 0,
     .osd_home_position_arm_screen = true,
     .pan_servo_index = 0,
-    .pan_servo_us2centideg = 0,
+    .pan_servo_pwm2centideg = 0,
 
     .units = OSD_UNIT_METRIC,
     .main_voltage_decimals = 1,
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 021113a0090..e260e14f21b 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -349,7 +349,7 @@ typedef struct osdConfig_s {
     uint8_t right_sidebar_scroll_step;  // Same as left_sidebar_scroll_step, but for the right sidebar.
     bool osd_home_position_arm_screen;
     uint8_t pan_servo_index;            // Index of the pan servo used for home direction offset
-    int8_t pan_servo_us2centideg;          // centidegrees per us pwm
+    int8_t pan_servo_pwm2centideg;      // Centidegrees of servo rotation per us pwm
     uint8_t crsf_lq_format;
 
 } osdConfig_t;

From be58a0457e7e7ad3a9490f55b484eac1278f8b88 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Sun, 31 Jan 2021 15:10:48 +0100
Subject: [PATCH 30/50] set min mag calibration time to 20 sec

---
 src/main/fc/settings.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index f5fde879efe..71af8c5f62e 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -461,7 +461,7 @@ groups:
         description: "Adjust how long time the Calibration of mag will last."
         default_value: "30"
         field: magCalibrationTimeLimit
-        min: 30
+        min: 20
         max: 120
       - name: mag_to_use
         description: "Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target"

From 8a8efbcbeebd3156a0f4a095c57ce53ad4634744 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 1 Feb 2021 14:58:53 +0100
Subject: [PATCH 31/50] Basic support for Rush Blade F7

---
 src/main/target/RUSH_BLADE_F7/CMakeLists.txt |   1 +
 src/main/target/RUSH_BLADE_F7/target.c       |  37 +++++
 src/main/target/RUSH_BLADE_F7/target.h       | 147 +++++++++++++++++++
 3 files changed, 185 insertions(+)
 create mode 100644 src/main/target/RUSH_BLADE_F7/CMakeLists.txt
 create mode 100644 src/main/target/RUSH_BLADE_F7/target.c
 create mode 100644 src/main/target/RUSH_BLADE_F7/target.h

diff --git a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt
new file mode 100644
index 00000000000..5dd7f2d2e1d
--- /dev/null
+++ b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f722xe(RUSH_BLADE_F7)
\ No newline at end of file
diff --git a/src/main/target/RUSH_BLADE_F7/target.c b/src/main/target/RUSH_BLADE_F7/target.c
new file mode 100644
index 00000000000..c56197a99e2
--- /dev/null
+++ b/src/main/target/RUSH_BLADE_F7/target.c
@@ -0,0 +1,37 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it 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.
+ *
+ * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include "platform.h"
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+
+const timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM8, CH3, PC8,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S1   UP2-1   D(2, 4, 7)
+    DEF_TIM(TIM8, CH4, PC9,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S2   UP2-1   D(2, 7, 7)
+    DEF_TIM(TIM3, CH4, PB1,     TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S6   UP1-2   D(1, 2, 5)
+    DEF_TIM(TIM3, CH3, PB0,     TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S5   UP1-2   D(1, 7, 5)
+    DEF_TIM(TIM8, CH2, PC7,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5 D(2, 3, 7)
+    DEF_TIM(TIM8, CH1, PC6,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6 D(1, 4, 5)
+
+    DEF_TIM(TIM2, CH1, PA15,    TIM_USE_LED, 0, 0),                            // LED 
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h
new file mode 100644
index 00000000000..ca0175f10df
--- /dev/null
+++ b/src/main/target/RUSH_BLADE_F7/target.h
@@ -0,0 +1,147 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it 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.
+ *
+ * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "RBF7"
+#define USBD_PRODUCT_STRING  "RUSH_BLADE_F7"
+
+#define LED0                    PB10  //Blue   SWCLK
+// #define LED1                    PA13  //Green  SWDIO
+
+#define BEEPER                  PB2
+#define BEEPER_INVERTED
+
+// *************** SPI1 Gyro & ACC *******************
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_EXTI
+#define USE_MPU_DATA_READY_SIGNAL
+#define GYRO_INT_EXTI           PA4
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
+#define MPU6000_CS_PIN          PC4
+#define MPU6000_SPI_BUS         BUS_SPI1
+
+// *************** I2C /Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_AK8975
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
+// *************** SPI2 Flash ***********************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS          BUS_SPI2
+#define M25P16_CS_PIN           PB12
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** UART *****************************
+#define USE_VCP
+#define USB_DETECT_PIN          PC14
+#define USE_USB_DETECT
+
+#define USE_UART1
+#define UART1_TX_PIN            PA9
+#define UART1_RX_PIN            PA10
+
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            PA3
+
+#define USE_UART3
+#define UART3_TX_PIN            PC10
+#define UART3_RX_PIN            PC11
+
+#define USE_UART4
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PA1
+
+#define USE_UART5
+#define UART5_TX_PIN            PC12
+#define UART5_RX_PIN            PD2
+
+#define SERIAL_PORT_COUNT       6
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+#define DEFAULT_FEATURES   (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL)
+#define CURRENT_METER_SCALE     179
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS        10
+#define USE_OSD
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR

From bae0e9b00c6b2152a817258dfb1adc4dea4bba36 Mon Sep 17 00:00:00 2001
From: Olivier C <olivierc600@gmail.com>
Date: Tue, 2 Feb 2021 09:15:41 +0100
Subject: [PATCH 32/50] Remove duplicate WP message in the OSD/Hud

... since it's now displayed in the system message ("TO WP 1/4")
---
 src/main/io/osd.c | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 3612ef00d5a..6dbe3189aac 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1762,9 +1762,6 @@ static bool osdDrawSingleElement(uint8_t item)
                 gpsLocation_t wp2;
                 int j;
 
-                tfp_sprintf(buff, "W%u/%u", posControl.activeWaypointIndex, posControl.waypointCount);
-                displayWrite(osdGetDisplayPort(), 13, osdConfig()->hud_margin_v - 1, buff);
-
                 for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
                     j = posControl.activeWaypointIndex + i;
                     if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) {

From 349f054bdaae83957d0e67a7d54c506ce5ab888b Mon Sep 17 00:00:00 2001
From: Harry Phillips <harryjamesphillips@gmail.com>
Date: Wed, 3 Feb 2021 13:19:42 +0000
Subject: [PATCH 33/50] Implement Prearm

---
 src/main/fc/fc_core.c        | 9 +++++++++
 src/main/fc/fc_msp_box.c     | 2 ++
 src/main/fc/rc_modes.h       | 1 +
 src/main/fc/runtime_config.c | 5 +++--
 src/main/fc/runtime_config.h | 6 +++---
 src/main/io/osd.c            | 2 ++
 src/main/io/osd.h            | 1 +
 src/main/io/osd_dji_hd.c     | 2 ++
 8 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 71f758dc408..48e941a0184 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -295,6 +295,14 @@ static void updateArmingStatus(void)
 	       DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
 	    }
 
+        if (isModeActivationConditionPresent(BOXPREARM)) {
+            if (IS_RC_MODE_ACTIVE(BOXPREARM)) {
+                DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
+            } else {
+                ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
+            }
+        }
+
         /* CHECK: Arming switch */
         // If arming is disabled and the ARM switch is on
         // Note that this should be last check so all other blockers could be cleared correctly
@@ -506,6 +514,7 @@ void tryArm(void)
 #else
         beeper(BEEPER_ARMING);
 #endif
+
         statsOnArm();
 
         return;
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 3afe82c9cbc..4189c4f8639 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 },
+    { BOXPREARM, "PREARM", 51 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -163,6 +164,7 @@ void initActiveBoxIds(void)
 
     activeBoxIdCount = 0;
     activeBoxIds[activeBoxIdCount++] = BOXARM;
+    activeBoxIds[activeBoxIdCount++] = BOXPREARM;
 
     if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
         activeBoxIds[activeBoxIdCount++] = BOXANGLE;
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 2058c822d31..a29a39bf1df 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,
+    BOXPREARM        = 42,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c
index 5ba8c6287e9..b0d014d25ef 100644
--- a/src/main/fc/runtime_config.c
+++ b/src/main/fc/runtime_config.c
@@ -36,7 +36,7 @@ const char *armingDisableFlagNames[]= {
     "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS",
     "ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX",
     "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM",
-    "SETTINGFAIL", "PWMOUT"
+    "SETTINGFAIL", "PWMOUT", "NOPREARM"
 };
 #endif
 
@@ -57,7 +57,8 @@ const armingFlag_e armDisableReasonsChecklist[] = {
     ARMING_DISABLED_OSD_MENU,
     ARMING_DISABLED_ROLLPITCH_NOT_CENTERED,
     ARMING_DISABLED_SERVO_AUTOTRIM,
-    ARMING_DISABLED_OOM
+    ARMING_DISABLED_OOM,
+    ARMING_DISABLED_NO_PREARM
 };
 
 armingFlag_e isArmingDisabledReason(void)
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index 7b28b464688..aeb7a99af41 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -23,7 +23,6 @@ typedef enum {
     WAS_EVER_ARMED                                  = (1 << 3),
 
     ARMING_DISABLED_FAILSAFE_SYSTEM                 = (1 << 7),
-
     ARMING_DISABLED_NOT_LEVEL                       = (1 << 8),
     ARMING_DISABLED_SENSORS_CALIBRATING             = (1 << 9),
     ARMING_DISABLED_SYSTEM_OVERLOADED               = (1 << 10),
@@ -44,6 +43,7 @@ typedef enum {
     ARMING_DISABLED_OOM                             = (1 << 25),
     ARMING_DISABLED_INVALID_SETTING                 = (1 << 26),
     ARMING_DISABLED_PWM_OUTPUT_ERROR                = (1 << 27),
+    ARMING_DISABLED_NO_PREARM                       = (1 << 28),
 
     ARMING_DISABLED_ALL_FLAGS                       = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | 
                                                        ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
@@ -52,7 +52,7 @@ typedef enum {
                                                        ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI |
                                                        ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED |
                                                        ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING |
-                                                       ARMING_DISABLED_PWM_OUTPUT_ERROR),
+                                                       ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM),
 } armingFlag_e;
 
 // Arming blockers that can be overriden by emergency arming.
@@ -78,7 +78,7 @@ extern const char *armingDisableFlagNames[];
 #define ARMING_FLAG(mask)           (armingFlags & (mask))
 
 // Returns the 1st flag from ARMING_DISABLED_ALL_FLAGS which is
-// preventing arming, or zero is arming is not disabled.
+// preventing arming, or zero if arming is not disabled.
 armingFlag_e isArmingDisabledReason(void);
 
 typedef enum {
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 3612ef00d5a..0cfc25a6f2d 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -712,6 +712,8 @@ static const char * osdArmingDisabledReasonMessage(void)
             return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE);
         case ARMING_DISABLED_PWM_OUTPUT_ERROR:
             return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR);
+        case ARMING_DISABLED_NO_PREARM:
+            return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM);
             // Cases without message
         case ARMING_DISABLED_CMS_MENU:
             FALLTHROUGH;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index d740b5de521..386d7bbcaab 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -76,6 +76,7 @@
 #define OSD_MSG_INVALID_SETTING     "INVALID SETTING"
 #define OSD_MSG_CLI_ACTIVE          "CLI IS ACTIVE"
 #define OSD_MSG_PWM_INIT_ERROR      "PWM INIT ERROR"
+#define OSD_MSG_NO_PREARM           "NO PREARM"
 #define OSD_MSG_RTH_FS              "(RTH)"
 #define OSD_MSG_EMERG_LANDING_FS    "(EMERGENCY LANDING)"
 #define OSD_MSG_MOVE_EXIT_FS        "!MOVE STICKS TO EXIT FS!"
diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c
index f506c8b9cf9..7da1e1542a7 100644
--- a/src/main/io/osd_dji_hd.c
+++ b/src/main/io/osd_dji_hd.c
@@ -500,6 +500,8 @@ static const char * osdArmingDisabledReasonMessage(void)
             return OSD_MESSAGE_STR("CLI");
         case ARMING_DISABLED_PWM_OUTPUT_ERROR:
             return OSD_MESSAGE_STR("PWM ERR");
+        case ARMING_DISABLED_NO_PREARM:
+            return OSD_MESSAGE_STR("NO PREARM");
             // Cases without message
         case ARMING_DISABLED_CMS_MENU:
             FALLTHROUGH;

From d9158e3ff520a558c810e37b2c8562f61f912776 Mon Sep 17 00:00:00 2001
From: Harry Phillips <harryjamesphillips@gmail.com>
Date: Wed, 3 Feb 2021 17:36:28 +0000
Subject: [PATCH 34/50] Optimize isModeActivationConditionPresent

---
 src/main/fc/rc_modes.c | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c
index 2336f6e965f..1bd12915f27 100755
--- a/src/main/fc/rc_modes.c
+++ b/src/main/fc/rc_modes.c
@@ -134,13 +134,7 @@ void rcModeUpdate(boxBitmask_t *newState)
 
 bool isModeActivationConditionPresent(boxId_e modeId)
 {
-    for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-        if (modeActivationConditions(index)->modeId == modeId && IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
-            return true;
-        }
-    }
-
-    return false;
+    return specifiedConditionCountPerMode[modeId] > 0;
 }
 
 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range)

From 0f5d6f4463ef18f028adae7decc35c2b2e343f3d Mon Sep 17 00:00:00 2001
From: Harry Phillips <harryjamesphillips@gmail.com>
Date: Wed, 3 Feb 2021 20:24:22 +0000
Subject: [PATCH 35/50] Handle edge case

---
 src/main/fc/fc_core.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 48e941a0184..b1fe71ca436 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -301,6 +301,8 @@ static void updateArmingStatus(void)
             } else {
                 ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
             }
+        } else {
+            DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
         }
 
         /* CHECK: Arming switch */

From c2e9dc9cd861e5d8ae605c6fbf5c4f2e591ee196 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 10 Feb 2021 10:49:38 +0100
Subject: [PATCH 36/50] Skip release for Rush Blade F722

---
 src/main/target/RUSH_BLADE_F7/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt
index 5dd7f2d2e1d..5d612d6c68b 100644
--- a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt
+++ b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt
@@ -1 +1 @@
-target_stm32f722xe(RUSH_BLADE_F7)
\ No newline at end of file
+target_stm32f722xe(RUSH_BLADE_F7 SKIP_RELEASES)
\ No newline at end of file

From 730e1a7354cdda5f2eefad40427e83181df4ff7a Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@users.noreply.github.com>
Date: Wed, 10 Feb 2021 20:01:04 +0100
Subject: [PATCH 37/50] Enable baro median filtering by default again (#6584)

It had been disabled by mistake in f0943875
---
 src/main/sensors/barometer.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index 3506e9361a3..bb1325aeeb9 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -56,7 +56,7 @@
 
 baro_t baro;                        // barometer access functions
 
-PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3);
 
 #ifdef USE_BARO
 #define BARO_HARDWARE_DEFAULT    BARO_AUTODETECT
@@ -65,7 +65,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER
 #endif
 PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
     .baro_hardware = BARO_HARDWARE_DEFAULT,
-    .use_median_filtering = 0,
+    .use_median_filtering = 1,
     .baro_calibration_tolerance = 150
 );
 

From 282c3b132e14c87a56d829a46dbc2e5d68f91e6c Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@gmail.com>
Date: Sun, 14 Feb 2021 17:07:30 +0100
Subject: [PATCH 38/50] Fix logic condition VSpeed constraint not allowing the
 value to be negative

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

diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
index d4465c4cec6..84e3b4607c8 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -418,7 +418,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
-            return constrain(getEstimatedActualVelocity(Z), 0, INT16_MAX);
+            return constrain(getEstimatedActualVelocity(Z), INT16_MIN, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %

From 9a6fd4a7eee2f6799dca22684f7b29b058ff3882 Mon Sep 17 00:00:00 2001
From: Alexander van Saase <avsaase@gmail.com>
Date: Mon, 15 Feb 2021 14:27:01 +0100
Subject: [PATCH 39/50] velocity in cm/s

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

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 94f612afa58..50ffb70fb73 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -569,7 +569,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         if (ctx->newFlags & EST_GPS_Z_VALID) {
             // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
             const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
-            const float gpsRocScaler = bellCurve(gpsRocResidual, 2.5f);
+            const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
             ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
         }
 

From c43109178055614002fc0642826dde726785930d Mon Sep 17 00:00:00 2001
From: OptimusTi <optimusti@yahoo.com>
Date: Mon, 15 Feb 2021 12:06:14 -0500
Subject: [PATCH 40/50] CRSF SNR Range update

This updates SNR range values to keep them inline with typical LoRa SNR values
---
 src/main/fc/settings.yaml |  4 ++--
 src/main/io/osd.c         | 10 +++++-----
 src/main/io/osd.h         |  4 ++--
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a49d69ff33d..ec2b1a01ea9 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2799,8 +2799,8 @@ groups:
         description: "Value below which Crossfire SNR Alarm pops-up. (dB)"
         default_value: "4"
         field: snr_alarm
-        min: -12
-        max: 8
+        min: -20
+        max: 10
       - name: osd_link_quality_alarm
         condition: USE_SERIALRX_CRSF
         description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%"
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 6dbe3189aac..4055cf733a6 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -785,7 +785,7 @@ static const char * navigationStateMessage(void)
             break;
         case MW_NAV_STATE_RTH_START:
             return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH);
-        case MW_NAV_STATE_RTH_CLIMB:            
+        case MW_NAV_STATE_RTH_CLIMB:
             return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB);
         case MW_NAV_STATE_RTH_ENROUTE:
             return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
@@ -1685,7 +1685,7 @@ static bool osdDrawSingleElement(uint8_t item)
         }
 
     case OSD_CRSF_SNR_DB: {
-        const char* showsnr = "-12";
+        const char* showsnr = "-20";
         const char* hidesnr = "     ";
         int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR;
         if (osdSNR_Alarm <= osdConfig()->snr_alarm) {
@@ -3378,7 +3378,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
                     if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
                         // Countdown display for remaining Waypoints
-                        tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount);                            
+                        tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount);
                         messages[messageCount++] = messageBuf;
                     } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
                         // WP hold time countdown in seconds
@@ -3388,8 +3388,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining);
                             messages[messageCount++] = messageBuf;
                         }
-                    } else {                            
-                        const char *navStateMessage = navigationStateMessage();                             
+                    } else {
+                        const char *navStateMessage = navigationStateMessage();
                         if (navStateMessage) {
                             messages[messageCount++] = navStateMessage;
                         }
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index d740b5de521..cff09933587 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -292,7 +292,7 @@ typedef struct osdConfig_s {
     float gforce_axis_alarm_min;
     float gforce_axis_alarm_max;
 #ifdef USE_SERIALRX_CRSF
-    int16_t snr_alarm; //CRSF SNR alarm in dB
+    int8_t snr_alarm; //CRSF SNR alarm in dB
     int8_t link_quality_alarm;
 #endif
 #ifdef USE_BARO
@@ -337,7 +337,7 @@ typedef struct osdConfig_s {
 
     bool osd_failsafe_switch_layout;
     uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE
-    uint8_t plus_code_short; 
+    uint8_t plus_code_short;
     uint8_t osd_ahi_style;
     uint8_t force_grid;                 // Force a pixel based OSD to use grid mode.
     uint8_t ahi_bordered;               // Only used by the AHI widget

From 84083689bb4e144149be7a2c3ef9501c05ca1ba4 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 16 Feb 2021 11:54:39 +0100
Subject: [PATCH 41/50] Allow to configure speed source for DJI OSD

---
 src/main/fc/settings.yaml              |  9 ++++++++
 src/main/io/osd.c                      |  7 ------
 src/main/io/osd_common.c               | 11 ++++++++++
 src/main/io/osd_common.h               |  5 +++++
 src/main/io/osd_dji_hd.c               | 30 +++++++++++++++++---------
 src/main/io/osd_dji_hd.h               |  7 ++++++
 src/main/programming/logic_condition.c |  3 ++-
 7 files changed, 54 insertions(+), 18 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index ec2b1a01ea9..f13d9253f59 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -152,6 +152,9 @@ tables:
   - name: djiOsdTempSource
     values: ["ESC", "IMU", "BARO"]
     enum: djiOsdTempSource_e
+  - name: djiOsdSpeedSource
+    values: ["GROUND", "3D", "AIR"]
+    enum: djiOsdSpeedSource_e
   - name: nav_overrides_motor_stop
     enum: navOverridesMotorStop_e
     values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"]
@@ -3249,3 +3252,9 @@ groups:
           field: esc_temperature_source
           table: djiOsdTempSource
           type: uint8_t
+        - name: dji_speed_source
+          description: "Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR"
+          default_value: "GROUND"
+          field: speedSource
+          table: djiOsdSpeedSource
+          type: uint8_t
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 4055cf733a6..a4d8f3cf0e2 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1118,13 +1118,6 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale)
     osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale);
 }
 
-static int16_t osdGet3DSpeed(void)
-{
-    int16_t vert_speed = getEstimatedActualVelocity(Z);
-    int16_t hor_speed = gpsSol.groundSpeed;
-    return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
-}
-
 #endif
 
 static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) {
diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c
index c7975652650..6d24dc41db1 100644
--- a/src/main/io/osd_common.c
+++ b/src/main/io/osd_common.c
@@ -36,6 +36,8 @@
 #include "io/osd_common.h"
 #include "io/osd_grid.h"
 
+#include "navigation/navigation.h"
+
 #if defined(USE_OSD)
 
 #define CANVAS_DEFAULT_GRID_ELEMENT_WIDTH OSD_CHAR_WIDTH
@@ -151,4 +153,13 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas)
     osdGridDrawSidebars(display);
 }
 
+#ifdef USE_GPS
+int16_t osdGet3DSpeed(void)
+{
+    int16_t vert_speed = getEstimatedActualVelocity(Z);
+    int16_t hor_speed = gpsSol.groundSpeed;
+    return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
+}
+#endif
+
 #endif
diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h
index 054f458a63d..2bed67d5693 100644
--- a/src/main/io/osd_common.h
+++ b/src/main/io/osd_common.h
@@ -82,3 +82,8 @@ void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, c
 // grid slots.
 void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading);
 void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas);
+
+#ifdef USE_GPS
+int16_t osdGet3DSpeed(void);
+#endif
+
diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c
index f506c8b9cf9..57438dac365 100644
--- a/src/main/io/osd_dji_hd.c
+++ b/src/main/io/osd_dji_hd.c
@@ -58,6 +58,7 @@
 #include "io/gps.h"
 #include "io/osd.h"
 #include "io/osd_dji_hd.h"
+#include "io/osd_common.h"
 
 #include "rx/rx.h"
 
@@ -68,6 +69,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/esc_sensor.h"
 #include "sensors/temperature.h"
+#include "sensors/pitotmeter.h"
 
 #include "msp/msp.h"
 #include "msp/msp_protocol.h"
@@ -118,11 +120,12 @@
  * but reuse the packet decoder to minimize code duplication
  */
 
-PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 2);
 PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
     .use_name_for_messages  = true,
     .esc_temperature_source = DJI_OSD_TEMP_ESC,
     .proto_workarounds = DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA,
+    .speedSource = DJI_OSD_SPEED_GROUND,
 );
 
 // External dependency on looptime
@@ -639,13 +642,6 @@ static int32_t osdConvertVelocityToUnit(int32_t vel)
     return -1;
 }
 
-static int16_t osdDJIGet3DSpeed(void)
-{
-    int16_t vert_speed = getEstimatedActualVelocity(Z);
-    int16_t hor_speed = gpsSol.groundSpeed;
-    return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
-}
-
 /**
  * Converts velocity into a string based on the current unit system.
  * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
@@ -766,7 +762,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
                 osdDJIFormatThrottlePosition(djibuf,true);
                 break;
             case 'S':
-                osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed());
+                osdDJIFormatVelocityStr(djibuf, osdGet3DSpeed());
                 break;
             case 'E':
                 osdDJIEfficiencyMahPerKM(djibuf);
@@ -1004,7 +1000,21 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
             sbufWriteU32(dst, gpsSol.llh.lat);
             sbufWriteU32(dst, gpsSol.llh.lon);
             sbufWriteU16(dst, gpsSol.llh.alt / 100);
-            sbufWriteU16(dst, gpsSol.groundSpeed);
+
+            int reportedSpeed = 0;
+            if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_GROUND) {
+                reportedSpeed = gpsSol.groundSpeed;
+            } else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_3D) {
+                reportedSpeed = osdGet3DSpeed();
+            } else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_AIR) {
+            #ifdef USE_PITOT
+                reportedSpeed = pitot.airSpeed;
+            #else
+                reportedSpeed = 0;
+            #endif
+            }
+
+            sbufWriteU16(dst, reportedSpeed);
             sbufWriteU16(dst, gpsSol.groundCourse);
             break;
 
diff --git a/src/main/io/osd_dji_hd.h b/src/main/io/osd_dji_hd.h
index d105b275b65..84053231673 100644
--- a/src/main/io/osd_dji_hd.h
+++ b/src/main/io/osd_dji_hd.h
@@ -68,6 +68,12 @@ enum djiOsdTempSource_e {
     DJI_OSD_TEMP_BARO   = 2
 };
 
+enum djiOsdSpeedSource_e {
+    DJI_OSD_SPEED_GROUND    = 0,
+    DJI_OSD_SPEED_3D        = 1,
+    DJI_OSD_SPEED_AIR       = 2
+};
+
 enum djiOsdProtoWorkarounds_e {
     DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA    = 1 << 0,
 };
@@ -76,6 +82,7 @@ typedef struct djiOsdConfig_s {
     uint8_t use_name_for_messages;
     uint8_t esc_temperature_source;
     uint8_t proto_workarounds;
+    uint8_t speedSource;
 } djiOsdConfig_t;
 
 PG_DECLARE(djiOsdConfig_t, djiOsdConfig);
diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
index d4465c4cec6..e806dacd7e8 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -44,6 +44,7 @@
 #include "flight/imu.h"
 #include "flight/pid.h"
 #include "drivers/io_port_expander.h"
+#include "io/osd_common.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
@@ -402,7 +403,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         //FIXME align with osdGet3DSpeed
         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s
-            return (int) sqrtf(sq(gpsSol.groundSpeed) + sq((int)getEstimatedActualVelocity(Z)));
+            return osdGet3DSpeed();
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s

From 1994ce7acbc3c5f3eb8c5f67e974225bbeac4e72 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 16 Feb 2021 11:58:38 +0100
Subject: [PATCH 42/50] Update Settings.md

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

diff --git a/docs/Settings.md b/docs/Settings.md
index e0a8c522421..7fcdddf11e2 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -64,6 +64,7 @@
 | disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. |
 | display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON |
 | dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature |
+| dji_speed_source | GROUND | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR |
 | dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance |
 | dji_workarounds |  | Enables workarounds for different versions of MSP protocol used |
 | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter |

From 34666c5c0a7cd3b351181cbdb2f65af2cbfa221f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 16 Feb 2021 12:07:28 +0100
Subject: [PATCH 43/50] Fix compile on no-OSD boards

---
 src/main/io/osd_common.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c
index 6d24dc41db1..d785eac9798 100644
--- a/src/main/io/osd_common.c
+++ b/src/main/io/osd_common.c
@@ -153,6 +153,8 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas)
     osdGridDrawSidebars(display);
 }
 
+#endif
+
 #ifdef USE_GPS
 int16_t osdGet3DSpeed(void)
 {
@@ -160,6 +162,4 @@ int16_t osdGet3DSpeed(void)
     int16_t hor_speed = gpsSol.groundSpeed;
     return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
 }
-#endif
-
-#endif
+#endif
\ No newline at end of file

From e61e5632d08dfb63c3ff4be3399a4b365254ef39 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 19 Feb 2021 10:30:37 +0100
Subject: [PATCH 44/50] Simplify the Iterm freeze routine

---
 src/main/flight/pid.c | 20 ++++++--------------
 1 file changed, 6 insertions(+), 14 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 493c2192f7c..1693aa7e8fc 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -81,7 +81,6 @@ typedef struct {
     // Rate integrator
     float errorGyroIf;
     float errorGyroIfLimit;
-    float errorGyroIfFrozen;
 
     // Used for ANGLE filtering (PT1, we don't need super-sharpness here)
     pt1Filter_t angleFilterState;
@@ -353,7 +352,6 @@ void pidResetErrorAccumulators(void)
     for (int axis = 0; axis < 3; axis++) {
         pidState[axis].errorGyroIf = 0.0f;
         pidState[axis].errorGyroIfLimit = 0.0f;
-        pidState[axis].errorGyroIfFrozen = 0.0f;
     }
 }
 
@@ -603,15 +601,6 @@ static void applyItermLimiting(pidState_t *pidState) {
     }
 }
 
-static void applyItermFreezing(pidState_t *pidState) {
-    if (pidState->itermFreezeActive) {
-        pidState->errorGyroIf = pidState->errorGyroIfFrozen;
-    } else 
-    {
-        pidState->errorGyroIfFrozen = pidState->errorGyroIf;
-    }
-}
-
 static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
     UNUSED(pidState);
     UNUSED(axis);
@@ -624,11 +613,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float newPTerm = pTermProcess(pidState, rateError, dT);
     const float newFFTerm = pidState->rateTarget * pidState->kFF;
 
-    // Calculate integral
-    pidState->errorGyroIf += rateError * pidState->kI * dT;
+    /*
+     * Integral should be updated only if axis Iterm is not frozen
+     */
+    if (!pidState->itermFreezeActive) {
+        pidState->errorGyroIf += rateError * pidState->kI * dT;
+    }
 
     applyItermLimiting(pidState);
-    applyItermFreezing(pidState);
 
     if (pidProfile()->fixedWingItermThrowLimit != 0) {
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);

From 8f5ac3e51f8cb239d77efc9abde9c4a3d2f83773 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 21 Feb 2021 10:38:39 +0100
Subject: [PATCH 45/50] Refactor PID value getter

---
 src/main/programming/pid.c | 2 +-
 src/main/programming/pid.h | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c
index 09916fbb1b3..8b284087ebd 100644
--- a/src/main/programming/pid.c
+++ b/src/main/programming/pid.c
@@ -110,7 +110,7 @@ void programmingPidInit(void)
     }
 }
 
-int programmingPidGetOutput(uint8_t i) {
+int32_t programmingPidGetOutput(uint8_t i) {
     return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output;
 }
 
diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h
index 8645de8f130..0d55b952c67 100644
--- a/src/main/programming/pid.h
+++ b/src/main/programming/pid.h
@@ -51,4 +51,4 @@ typedef struct programmingPidState_s {
 void programmingPidUpdateTask(timeUs_t currentTimeUs);
 void programmingPidInit(void);
 void programmingPidReset(void);
-int programmingPidGetOutput(uint8_t i);
\ No newline at end of file
+int32_t programmingPidGetOutput(uint8_t i);
\ No newline at end of file

From 67572fed9d776eff58f781d4bedd29f8f3aab8d5 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 21 Feb 2021 11:20:07 +0100
Subject: [PATCH 46/50] Add ability to read Programming PID status via MSP

---
 src/main/fc/fc_msp.c                | 5 +++++
 src/main/msp/msp_protocol_v2_inav.h | 2 +-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index a32961dabae..0e47385044d 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -567,6 +567,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU16(dst, programmingPids(i)->gains.FF);
         }
         break;
+    case MSP2_INAV_PROGRAMMING_PID_STATUS:
+        for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+            sbufWriteU32(dst, programmingPidGetOutput(i));
+        }
+        break;
 #endif
     case MSP2_COMMON_MOTOR_MIXER:
         for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 70e394ccb3f..d3e7d14585a 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -63,6 +63,7 @@
 #define MSP2_INAV_GVAR_STATUS                   0x2027
 #define MSP2_INAV_PROGRAMMING_PID               0x2028
 #define MSP2_INAV_SET_PROGRAMMING_PID           0x2029
+#define MSP2_INAV_PROGRAMMING_PID_STATUS        0x202A
 
 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031
@@ -79,4 +80,3 @@
 #define MSP2_INAV_SET_SAFEHOME                  0x2039
 
 #define MSP2_INAV_MISC2                         0x203A
-

From 1a5133fa11d6f1e1d00482d96a3c57d9d161a67d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 24 Feb 2021 11:16:24 +0100
Subject: [PATCH 47/50] Fix mag gain computation for negative mag zero

---
 src/main/sensors/compass.c | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index 90a108109d6..adfc8e79a3d 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -350,7 +350,7 @@ void compassUpdate(timeUs_t currentTimeUs)
     static sensorCalibrationState_t calState;
     static timeUs_t calStartedAt = 0;
     static int16_t magPrev[XYZ_AXIS_COUNT];
-    static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096};
+    static int magAxisDeviation[XYZ_AXIS_COUNT];
 
     // Check magZero
     if (
@@ -381,6 +381,7 @@ void compassUpdate(timeUs_t currentTimeUs)
             compassConfigMutable()->magZero.raw[axis] = 0;
             compassConfigMutable()->magGain[axis] = 1024;
             magPrev[axis] = 0;
+            magAxisDeviation[axis] = 0;  // Gain is based on the biggest absolute deviation from the mag zero point. Gain computation starts at 0
         }
 
         beeper(BEEPER_ACTION_SUCCESS);
@@ -400,9 +401,9 @@ void compassUpdate(timeUs_t currentTimeUs)
                 diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]);
                 avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f;
 
-                const int32_t sample = ABS(mag.magADC[axis]);
-                if (sample > magGain[axis]) {
-                    magGain[axis] = sample;
+                // Find the biggest sample deviation together with sample' sign
+                if (ABS(mag.magADC[axis]) > ABS(magAxisDeviation[axis])) {
+                    magAxisDeviation[axis] = mag.magADC[axis];
                 }
 
             }
@@ -429,7 +430,7 @@ void compassUpdate(timeUs_t currentTimeUs)
              * It is dirty, but worth checking if this will solve the problem of changing mag vector when UAV is tilted
              */
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                compassConfigMutable()->magGain[axis] = magGain[axis] - compassConfig()->magZero.raw[axis];
+                compassConfigMutable()->magGain[axis] = ABS(magAxisDeviation[axis] - compassConfig()->magZero.raw[axis]);
             }
 
             calStartedAt = 0;

From 0b13c2cd8d0b2d83cf3539a19d78a64ad83d9ee0 Mon Sep 17 00:00:00 2001
From: Tim O'Brien <timo@t413.com>
Date: Wed, 24 Feb 2021 13:59:47 -0800
Subject: [PATCH 48/50] Adds heading target logic condition

- allows for a slider/global-var to set the desired heading
- addresses issue #6642
---
 src/main/programming/logic_condition.c | 6 ++++++
 src/main/programming/logic_condition.h | 3 ++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
index 84e3b4607c8..3c256531e25 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -317,6 +317,12 @@ static int logicConditionCompute(
             return true;
         break;
 
+        case LOGIC_CONDITION_SET_HEADING_TARGET:
+            temporaryValue = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(operandA)));
+            updateHeadingHoldTarget(temporaryValue);
+            return temporaryValue;
+        break;
+
         default:
             return false;
             break; 
diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
index fc38839d71e..f6d68307038 100644
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@@ -68,7 +68,8 @@ typedef enum {
     LOGIC_CONDITION_MAP_INPUT                   = 36,
     LOGIC_CONDITION_MAP_OUTPUT                  = 37,
     LOGIC_CONDITION_RC_CHANNEL_OVERRIDE         = 38,
-    LOGIC_CONDITION_LAST                        = 39,
+    LOGIC_CONDITION_SET_HEADING_TARGET          = 39,
+    LOGIC_CONDITION_LAST                        = 40,
 } logicOperation_e;
 
 typedef enum logicOperandType_s {

From 25e1524dea9504d164178519cd1252250ad58424 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 25 Feb 2021 11:40:32 +0100
Subject: [PATCH 49/50] Update docs for Lua telemetry

---
 README.md         | 2 +-
 docs/Telemetry.md | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/README.md b/README.md
index cbe75a1b189..f34db4d1361 100644
--- a/README.md
+++ b/README.md
@@ -39,7 +39,7 @@ Tool for Blackbox logs analysis is available [here](https://github.com/iNavFligh
 
 ### Telemetry screen for OpenTX
 
-Users of FrSky Taranis X9 and Q X7 can use INAV Lua Telemetry screen created by @teckel12 . Software and installation instruction are available here: [https://github.com/iNavFlight/LuaTelemetry](https://github.com/iNavFlight/LuaTelemetry)
+Users of OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget)
 
 ## Installation
 
diff --git a/docs/Telemetry.md b/docs/Telemetry.md
index 8c8b406ccf8..2b5972370e0 100644
--- a/docs/Telemetry.md
+++ b/docs/Telemetry.md
@@ -123,7 +123,7 @@ The following sensors are transmitted
 * **0450** : 'Flight Path Vector' or 'Course over ground' in degrees*10
 ### Compatible SmartPort/INAV telemetry flight status
 
-To quickly and easily monitor these SmartPort sensors and flight modes, install [iNav LuaTelemetry](https://github.com/iNavFlight/LuaTelemetry) to your Taranis Q X7, X9D, X9D+ or X9E transmitter.
+To quickly and easily monitor these SmartPort sensors and flight modes, install [OpenTX Telemetry Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget) to your Taranis Q X7, X9D, X9D+ or X9E transmitter.
 
 ## FrSky telemetry
 

From 0cb632263d57b9694495648fb764f688d45acd20 Mon Sep 17 00:00:00 2001
From: Tim O'Brien <timo@t413.com>
Date: Thu, 25 Feb 2021 11:14:42 -0800
Subject: [PATCH 50/50] Adds LC heading documentation + RC override docs

- also adds docs for the new RC overrides #6341
---
 docs/Programming Framework.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index 6e658058d1a..8d2297a97dd 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -71,6 +71,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 35            | TAN                | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
 | 36            | MAP_INPUT          | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
 | 37            | MAP_OUTPUT         | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
+| 38            | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
+| 39            | SET_HEADING_TARGET  | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
 
 
 ### Operands