Merge branch 'master' into dzikuvx-autolevel

2025-07-23 16:25:26 +03:00 · 2021-03-13 13:25:47 +01:00 · 2021-03-13 13:25:47 +01:00 · 703cbbbf5a
commit 703cbbbf5a
parent 9cbecaa45f ad3dfacb75
54 changed files with 939 additions and 271 deletions
--- a/1
+++ b/1
@ -7,6 +7,7 @@ Alberto García Hierro
 Alex Gorbatchev
 Alex Zaitsev
 Alexander Fedorov
+Alexander van Saase
 Alexey Stankevich
 Andre Bernet
 Andreas Tacke
--- 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

--- a/docs/Programming
+++ b/docs/Programming
@ -71,6 +71,9 @@ 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. |
+| 40            | MOD           | Divide `Operand A` by `Operand B` and returns the remainder |


 ### Operands
@ -93,8 +96,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 1             | HOME_DISTANCE | in `meters`                           |
 | 2             | TRIP_DISTANCE | in `meters`                           |
 | 3             | RSSI          |                                       |
-| 4             | VBAT          | in `Volts * 10`, eg. `12.1V` is `121` |
-| 5             | CELL_VOLTAGE  | in `Volts * 10`, eg. `12.1V` is `121` |
+| 4             | VBAT          | in `Volts * 100`, eg. `12.1V` is `1210` |
+| 5             | CELL_VOLTAGE  | in `Volts * 100`, eg. `12.1V` is `1210` |
 | 6             | CURRENT       | in `Amps * 100`, eg. `9A` is `900`    |
 | 7             | MAH_DRAWN     | in `mAh`                              |
 | 8             | GPS_SATS      |                                       |
--- 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 |
@ -81,7 +82,7 @@
 | eleres_signature |  |  |
 | eleres_telemetry_en |  |  |
 | eleres_telemetry_power |  |  |
-| esc_sensor_listen_only |  |  |
+| esc_sensor_listen_only | OFF | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case |
 | failsafe_delay | 5 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). |
 | failsafe_fw_pitch_angle | 100 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb |
 | failsafe_fw_roll_angle | -200 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll |
@ -113,6 +114,9 @@
 | fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection |
 | fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot |
 | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point |
+| fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH |
+| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL |
+| fw_d_yaw | 0 | Fixed wing rate stabilisation D-gain for YAW |
 | fw_ff_pitch | 50 | Fixed-wing rate stabilisation FF-gain for PITCH |
 | fw_ff_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL |
 | fw_ff_yaw | 60 | Fixed-wing rate stabilisation FF-gain for YAW |
@ -135,6 +139,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. |
@ -145,7 +150,7 @@
 | gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
 | gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF |
 | gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF |
-| gyro_hardware_lpf | 42HZ | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. |
+| gyro_hardware_lpf | 256HZ | Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. |
 | gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
 | gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
 | gyro_notch_cutoff |  |  |
@ -247,7 +252,7 @@
 | motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] |
 | motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] |
 | motor_direction_inverted | OFF | Use if you need to inverse yaw motor direction. |
-| motor_poles |  |  |
+| motor_poles | 14 | The number of motor poles. Required to compute motor RPM |
 | motor_pwm_protocol | ONESHOT125 | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED |
 | motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. |
 | msp_override_channels | 0 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. |
@ -302,7 +307,7 @@
 | nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] |
 | nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] |
 | nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
-| nav_max_terrain_follow_alt |  |  |
+| nav_max_terrain_follow_alt | 100 | Max allowed above the ground altitude for terrain following mode |
 | nav_mc_auto_disarm_delay | 2000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) |
 | nav_mc_bank_angle | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude |
 | nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode |
@ -330,6 +335,7 @@
 | nav_mc_vel_z_d | 10 | D gain of velocity PID controller |
 | nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
 | nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
+| nav_mc_wp_slowdown | ON | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. |
 | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
 | nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
 | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
@ -392,6 +398,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_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 |  |  |
@ -409,10 +417,10 @@
 | pid_type |  | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` |
 | pidsum_limit | 500 | 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 |
 | pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
-| pinio_box1 |  |  |
-| pinio_box2 |  |  |
-| pinio_box3 |  |  |
-| pinio_box4 |  |  |
+| pinio_box1 | target specific | Mode assignment for PINIO#1 |
+| pinio_box2 | target specific | Mode assignment for PINIO#1 |
+| pinio_box3 | target specific | Mode assignment for PINIO#1 |
+| pinio_box4 | target specific | Mode assignment for PINIO#1 |
 | pitch_rate | 20 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
 | pitot_hardware | NONE | Selection of pitot hardware. |
 | pitot_lpf_milli_hz |  |  |
--- 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

--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@ -73,6 +73,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
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@ -81,5 +81,6 @@ typedef enum {
    DEBUG_PCF8574,
    DEBUG_DYNAMIC_GYRO_LPF,
    DEBUG_AUTOLEVEL,
+    DEBUG_FW_D,
    DEBUG_COUNT
 } debugType_e;
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@ -55,7 +55,7 @@
 #define RPY_PIDFF_MAX 200
 #define OTHER_PIDDF_MAX 255

-#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
+#define PIDFF_ENTRY(label, ptr, max) OSD_UINT16_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
 #define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX)
 #define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX)

--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@ -142,7 +142,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
    return value;
 }

-int constrain(int amt, int low, int high)
+int32_t constrain(int32_t amt, int32_t low, int32_t high)
 {
    if (amt < low)
        return low;
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@ -131,7 +131,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
 int gcd(int num, int denom);
 int32_t applyDeadband(int32_t value, int32_t deadband);

-int constrain(int amt, int low, int high);
+int32_t constrain(int32_t amt, int32_t low, int32_t high);
 float constrainf(float amt, float low, float high);

 void devClear(stdev_t *dev);
--- a/src/main/drivers/accgyro/accgyro_bmi088.c
+++ 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 */
--- a/src/main/drivers/accgyro/accgyro_bmi088.h
+++ 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);
--- 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 */
--- a/src/main/drivers/lights_io.c
+++ b/src/main/drivers/lights_io.c
@ -10,7 +10,7 @@

 static IO_t lightsIO = DEFIO_IO(NONE);

-bool lightsHardwareInit()
+bool lightsHardwareInit(void)
 {
    lightsIO = IOGetByTag(IO_TAG(LIGHTS_PIN));

--- a/src/main/drivers/lights_io.h
+++ b/src/main/drivers/lights_io.h
@ -4,7 +4,7 @@
 #ifdef USE_LIGHTS


-bool lightsHardwareInit();
+bool lightsHardwareInit(void);
 void lightsHardwareSetStatus(bool status);

 #endif /* USE_LIGHTS */
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@ -295,6 +295,16 @@ 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);
+            }
+        } else {
+            DISABLE_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
@ -391,7 +401,9 @@ void disarm(disarmReason_t disarmReason)

        statsOnDisarm();
        logicConditionReset();
+#ifdef USE_PROGRAMMING_FRAMEWORK	    
        programmingPidReset();
+#endif	    
        beeper(BEEPER_DISARMING);      // emit disarm tone
    }
 }
@ -482,7 +494,10 @@ void tryArm(void)
        //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
        ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
        logicConditionReset();
+	    
+#ifdef USE_PROGRAMMING_FRAMEWORK	    
        programmingPidReset();
+#endif	    
        headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);

        resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
@ -506,6 +521,7 @@ void tryArm(void)
 #else
        beeper(BEEPER_ARMING);
 #endif
+
        statsOnArm();

        return;
--- 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++) {
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@ -87,6 +87,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
    { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
    { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
    { BOXAUTOLEVEL, "AUTO LEVEL", 51 },
+    { BOXPREARM, "PREARM", 52 },
    { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };

@ -164,6 +165,7 @@ void initActiveBoxIds(void)

    activeBoxIdCount = 0;
    activeBoxIds[activeBoxIdCount++] = BOXARM;
+    activeBoxIds[activeBoxIdCount++] = BOXPREARM;

    if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
        activeBoxIds[activeBoxIdCount++] = BOXANGLE;
--- a/src/main/fc/rc_adjustments.c
+++ b/src/main/fc/rc_adjustments.c
@ -101,7 +101,43 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
        .mode = ADJUSTMENT_MODE_STEP,
        .data = { .stepConfig = { .step = 1 }}
    }, {
-        .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D_FF,
+        .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_FF,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_PITCH_P,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_PITCH_I,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_PITCH_D,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_PITCH_FF,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_ROLL_P,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_ROLL_I,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_ROLL_D,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_ROLL_FF,
        .mode = ADJUSTMENT_MODE_STEP,
        .data = { .stepConfig = { .step = 1 }}
    }, {
@ -113,7 +149,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
        .mode = ADJUSTMENT_MODE_STEP,
        .data = { .stepConfig = { .step = 1 }}
    }, {
-        .adjustmentFunction = ADJUSTMENT_YAW_D_FF,
+        .adjustmentFunction = ADJUSTMENT_YAW_D,
+        .mode = ADJUSTMENT_MODE_STEP,
+        .data = { .stepConfig = { .step = 1 }}
+    }, {
+        .adjustmentFunction = ADJUSTMENT_YAW_FF,
        .mode = ADJUSTMENT_MODE_STEP,
        .data = { .stepConfig = { .step = 1 }}
    }, {
@ -128,30 +168,6 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
        .adjustmentFunction = ADJUSTMENT_ROLL_RATE,
        .mode = ADJUSTMENT_MODE_STEP,
        .data = { .stepConfig = { .step = 1 }}
-    }, {
-        .adjustmentFunction = ADJUSTMENT_PITCH_P,
-        .mode = ADJUSTMENT_MODE_STEP,
-        .data = { .stepConfig = { .step = 1 }}
-    }, {
-        .adjustmentFunction = ADJUSTMENT_PITCH_I,
-        .mode = ADJUSTMENT_MODE_STEP,
-        .data = { .stepConfig = { .step = 1 }}
-    }, {
-        .adjustmentFunction = ADJUSTMENT_PITCH_D_FF,
-        .mode = ADJUSTMENT_MODE_STEP,
-        .data = { .stepConfig = { .step = 1 }}
-    }, {
-        .adjustmentFunction = ADJUSTMENT_ROLL_P,
-        .mode = ADJUSTMENT_MODE_STEP,
-        .data = { .stepConfig = { .step = 1 }}
-    }, {
-        .adjustmentFunction = ADJUSTMENT_ROLL_I,
-        .mode = ADJUSTMENT_MODE_STEP,
-        .data = { .stepConfig = { .step = 1 }}
-    }, {
-        .adjustmentFunction = ADJUSTMENT_ROLL_D_FF,
-        .mode = ADJUSTMENT_MODE_STEP,
-        .data = { .stepConfig = { .step = 1 }}
    }, {
        .adjustmentFunction = ADJUSTMENT_RC_YAW_EXPO,
        .mode = ADJUSTMENT_MODE_STEP,
@ -447,18 +463,32 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
            applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta);
            schedulePidGainsUpdate();
            break;
-        case ADJUSTMENT_PITCH_ROLL_D_FF:
-        case ADJUSTMENT_PITCH_D_FF:
-            applyAdjustmentPID(ADJUSTMENT_PITCH_D_FF, getD_FFRefByBank(pidBankMutable(), PID_PITCH), delta);
-            if (adjustmentFunction == ADJUSTMENT_PITCH_D_FF) {
+        case ADJUSTMENT_PITCH_ROLL_D:
+        case ADJUSTMENT_PITCH_D:
+            applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta);
+            if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
                schedulePidGainsUpdate();
                break;
            }
            // follow though for combined ADJUSTMENT_PITCH_ROLL_D
            FALLTHROUGH;

-        case ADJUSTMENT_ROLL_D_FF:
-            applyAdjustmentPID(ADJUSTMENT_ROLL_D_FF, getD_FFRefByBank(pidBankMutable(), PID_ROLL), delta);
+        case ADJUSTMENT_ROLL_D:
+            applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta);
+            schedulePidGainsUpdate();
+            break;
+        case ADJUSTMENT_PITCH_ROLL_FF:
+        case ADJUSTMENT_PITCH_FF:
+            applyAdjustmentPID(ADJUSTMENT_PITCH_FF, &pidBankMutable()->pid[PID_PITCH].FF, delta);
+            if (adjustmentFunction == ADJUSTMENT_PITCH_FF) {
+                schedulePidGainsUpdate();
+                break;
+            }
+            // follow though for combined ADJUSTMENT_PITCH_ROLL_FF
+            FALLTHROUGH;
+
+        case ADJUSTMENT_ROLL_FF:
+            applyAdjustmentPID(ADJUSTMENT_ROLL_FF, &pidBankMutable()->pid[PID_ROLL].FF, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_YAW_P:
@ -469,8 +499,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
            applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta);
            schedulePidGainsUpdate();
            break;
-        case ADJUSTMENT_YAW_D_FF:
-            applyAdjustmentPID(ADJUSTMENT_YAW_D_FF, getD_FFRefByBank(pidBankMutable(), PID_YAW), delta);
+        case ADJUSTMENT_YAW_D:
+            applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta);
+            schedulePidGainsUpdate();
+            break;
+        case ADJUSTMENT_YAW_FF:
+            applyAdjustmentPID(ADJUSTMENT_YAW_FF, &pidBankMutable()->pid[PID_YAW].FF, delta);
            schedulePidGainsUpdate();
            break;
        case ADJUSTMENT_NAV_FW_CRUISE_THR:
--- a/src/main/fc/rc_adjustments.h
+++ b/src/main/fc/rc_adjustments.h
@ -33,51 +33,55 @@ typedef enum {
    ADJUSTMENT_YAW_RATE                 = 5,
    ADJUSTMENT_PITCH_ROLL_P             = 6,
    ADJUSTMENT_PITCH_ROLL_I             = 7,
-    ADJUSTMENT_PITCH_ROLL_D_FF          = 8,
-    ADJUSTMENT_YAW_P                    = 9,
-    ADJUSTMENT_YAW_I                    = 10,
-    ADJUSTMENT_YAW_D_FF                 = 11,
-    ADJUSTMENT_RATE_PROFILE             = 12,    // Unused, placeholder for compatibility
-    ADJUSTMENT_PITCH_RATE               = 13,
-    ADJUSTMENT_ROLL_RATE                = 14,
-    ADJUSTMENT_PITCH_P                  = 15,
-    ADJUSTMENT_PITCH_I                  = 16,
-    ADJUSTMENT_PITCH_D_FF               = 17,
-    ADJUSTMENT_ROLL_P                   = 18,
-    ADJUSTMENT_ROLL_I                   = 19,
-    ADJUSTMENT_ROLL_D_FF                = 20,
-    ADJUSTMENT_RC_YAW_EXPO              = 21,
-    ADJUSTMENT_MANUAL_RC_EXPO           = 22,
-    ADJUSTMENT_MANUAL_RC_YAW_EXPO       = 23,
-    ADJUSTMENT_MANUAL_PITCH_ROLL_RATE   = 24,
-    ADJUSTMENT_MANUAL_ROLL_RATE         = 25,
-    ADJUSTMENT_MANUAL_PITCH_RATE        = 26,
-    ADJUSTMENT_MANUAL_YAW_RATE          = 27,
-    ADJUSTMENT_NAV_FW_CRUISE_THR        = 28,
-    ADJUSTMENT_NAV_FW_PITCH2THR         = 29,
-    ADJUSTMENT_ROLL_BOARD_ALIGNMENT     = 30,
-    ADJUSTMENT_PITCH_BOARD_ALIGNMENT    = 31,
-    ADJUSTMENT_LEVEL_P                  = 32,
-    ADJUSTMENT_LEVEL_I                  = 33,
-    ADJUSTMENT_LEVEL_D                  = 34,
-    ADJUSTMENT_POS_XY_P                 = 35,
-    ADJUSTMENT_POS_XY_I                 = 36,
-    ADJUSTMENT_POS_XY_D                 = 37,
-    ADJUSTMENT_POS_Z_P                  = 38,
-    ADJUSTMENT_POS_Z_I                  = 39,
-    ADJUSTMENT_POS_Z_D                  = 40,
-    ADJUSTMENT_HEADING_P                = 41,
-    ADJUSTMENT_VEL_XY_P                 = 42,
-    ADJUSTMENT_VEL_XY_I                 = 43,
-    ADJUSTMENT_VEL_XY_D                 = 44,
-    ADJUSTMENT_VEL_Z_P                  = 45,
-    ADJUSTMENT_VEL_Z_I                  = 46,
-    ADJUSTMENT_VEL_Z_D                  = 47,
-    ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48,
-    ADJUSTMENT_VTX_POWER_LEVEL = 49,
-    ADJUSTMENT_TPA                      = 50,
-    ADJUSTMENT_TPA_BREAKPOINT           = 51,
-    ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 52,
+    ADJUSTMENT_PITCH_ROLL_D             = 8,
+    ADJUSTMENT_PITCH_ROLL_FF            = 9,
+    ADJUSTMENT_PITCH_P                  = 10,
+    ADJUSTMENT_PITCH_I                  = 11,
+    ADJUSTMENT_PITCH_D                  = 12,
+    ADJUSTMENT_PITCH_FF                 = 13,
+    ADJUSTMENT_ROLL_P                   = 14,
+    ADJUSTMENT_ROLL_I                   = 15,
+    ADJUSTMENT_ROLL_D                   = 16,
+    ADJUSTMENT_ROLL_FF                  = 17,
+    ADJUSTMENT_YAW_P                    = 18,
+    ADJUSTMENT_YAW_I                    = 19,
+    ADJUSTMENT_YAW_D                    = 20,
+    ADJUSTMENT_YAW_FF                   = 21,
+    ADJUSTMENT_RATE_PROFILE             = 22,    // Unused, placeholder for compatibility
+    ADJUSTMENT_PITCH_RATE               = 23,
+    ADJUSTMENT_ROLL_RATE                = 24,
+    ADJUSTMENT_RC_YAW_EXPO              = 25,
+    ADJUSTMENT_MANUAL_RC_EXPO           = 26,
+    ADJUSTMENT_MANUAL_RC_YAW_EXPO       = 27,
+    ADJUSTMENT_MANUAL_PITCH_ROLL_RATE   = 28,
+    ADJUSTMENT_MANUAL_ROLL_RATE         = 29,
+    ADJUSTMENT_MANUAL_PITCH_RATE        = 30,
+    ADJUSTMENT_MANUAL_YAW_RATE          = 31,
+    ADJUSTMENT_NAV_FW_CRUISE_THR        = 32,
+    ADJUSTMENT_NAV_FW_PITCH2THR         = 33,
+    ADJUSTMENT_ROLL_BOARD_ALIGNMENT     = 34,
+    ADJUSTMENT_PITCH_BOARD_ALIGNMENT    = 35,
+    ADJUSTMENT_LEVEL_P                  = 36,
+    ADJUSTMENT_LEVEL_I                  = 37,
+    ADJUSTMENT_LEVEL_D                  = 38,
+    ADJUSTMENT_POS_XY_P                 = 39,
+    ADJUSTMENT_POS_XY_I                 = 40,
+    ADJUSTMENT_POS_XY_D                 = 41,
+    ADJUSTMENT_POS_Z_P                  = 42,
+    ADJUSTMENT_POS_Z_I                  = 43,
+    ADJUSTMENT_POS_Z_D                  = 44,
+    ADJUSTMENT_HEADING_P                = 45,
+    ADJUSTMENT_VEL_XY_P                 = 46,
+    ADJUSTMENT_VEL_XY_I                 = 47,
+    ADJUSTMENT_VEL_XY_D                 = 48,
+    ADJUSTMENT_VEL_Z_P                  = 49,
+    ADJUSTMENT_VEL_Z_I                  = 50,
+    ADJUSTMENT_VEL_Z_D                  = 51,
+    ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 52,
+    ADJUSTMENT_VTX_POWER_LEVEL          = 53,
+    ADJUSTMENT_TPA                      = 54,
+    ADJUSTMENT_TPA_BREAKPOINT           = 55,
+    ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 56,
    ADJUSTMENT_FUNCTION_COUNT // must be last
 } adjustmentFunction_e;

--- 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)
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@ -69,6 +69,7 @@ typedef enum {
    BOXLOITERDIRCHN  = 40,
    BOXMSPRCOVERRIDE = 41,
    BOXAUTOLEVEL     = 42,
+    BOXPREARM        = 43,
    CHECKBOX_ITEM_COUNT
 } boxId_e;

--- 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)
--- 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 {
--- 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", "UNUSED", "BENEWAKE"]
@ -13,7 +13,7 @@ tables:
    values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"]
    enum: magSensor_e
  - name: opflow_hardware
-    values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"]
+    values: ["NONE", "CXOF", "MSP", "FAKE"]
    enum: opticalFlowSensor_e
  - name: baro_hardware
    values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "MSP", "FAKE"]
@ -84,7 +84,7 @@ tables:
      "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
-      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "FW_D"]
  - name: async_mode
    values: ["NONE", "GYRO", "ALL"]
  - name: aux_operator
@ -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"]
@ -179,8 +182,8 @@ groups:
        type: uint8_t
        table: alignment
      - name: gyro_hardware_lpf
-        description: "Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first."
-        default_value: "42HZ"
+        description: "Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first."
+        default_value: "256HZ"
        field: gyro_lpf
        table: gyro_lpf
      - name: gyro_lpf_hz
@ -461,7 +464,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"
@ -715,6 +718,8 @@ groups:
        max: 30
      - name: motor_poles
        field: motorPoleCount
+        description: "The number of motor poles. Required to compute motor RPM"
+        default_value: "14"
        min: 4
        max: 255

@ -1461,6 +1466,12 @@ groups:
        field: bank_fw.pid[PID_PITCH].I
        min: 0
        max: 200
+      - name: fw_d_pitch
+        description: "Fixed wing rate stabilisation D-gain for PITCH"
+        default_value: "0"
+        field: bank_fw.pid[PID_PITCH].D
+        min: 0
+        max: 200
      - name: fw_ff_pitch
        description: "Fixed-wing rate stabilisation FF-gain for PITCH"
        default_value: "50"
@ -1479,6 +1490,12 @@ groups:
        field: bank_fw.pid[PID_ROLL].I
        min: 0
        max: 200
+      - name: fw_d_roll
+        description: "Fixed wing rate stabilisation D-gain for ROLL"
+        default_value: "0"
+        field: bank_fw.pid[PID_ROLL].D
+        min: 0
+        max: 200
      - name: fw_ff_roll
        description: "Fixed-wing rate stabilisation FF-gain for ROLL"
        default_value: "50"
@ -1497,6 +1514,12 @@ groups:
        field: bank_fw.pid[PID_YAW].I
        min: 0
        max: 200
+      - name: fw_d_yaw
+        description: "Fixed wing rate stabilisation D-gain for YAW"
+        default_value: "0"
+        field: bank_fw.pid[PID_YAW].D
+        min: 0
+        max: 200
      - name: fw_ff_yaw
        description: "Fixed-wing rate stabilisation FF-gain for YAW"
        default_value: "60"
@ -1594,6 +1617,12 @@ groups:
        field: fixedWingItermLimitOnStickPosition
        min: 0
        max: 1
+      - 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: fixedWingYawItermBankFreeze
+        min: 0
+        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"
@ -2186,6 +2215,8 @@ groups:
        max: 65000
      - name: nav_max_terrain_follow_alt
        field: general.max_terrain_follow_altitude
+        default_value: "100"
+        description: "Max allowed above the ground altitude for terrain following mode"
        max: 1000
      - name: nav_rth_altitude
        description: "Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters)"
@ -2291,6 +2322,11 @@ groups:
        condition: USE_NAV
        min: 0
        max: 255
+      - name: nav_mc_wp_slowdown
+        description: "When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes."
+        default_value: "ON"
+        field: mc.slowDownForTurning
+        type: bool
      - name: nav_fw_cruise_thr
        description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )"
        default_value: "1400"
@ -3033,6 +3069,20 @@ groups:
        default_value: "ON"
        description: Should home position coordinates be displayed on the arming screen.

+      - name: osd_pan_servo_index
+        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_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
+
+
  - name: PG_SYSTEM_CONFIG
    type: systemConfig_t
    headers: ["fc/config.h"]
@ -3183,21 +3233,29 @@ groups:
    members:
      - name: pinio_box1
        field: permanentId[0]
+        description: "Mode assignment for PINIO#1"
+        default_value: "target specific"
        min: 0
        max: 255
        type: uint8_t
      - name: pinio_box2
        field: permanentId[1]
+        default_value: "target specific"
+        description: "Mode assignment for PINIO#1"
        min: 0
        max: 255
        type: uint8_t
      - name: pinio_box3
        field: permanentId[2]
+        default_value: "target specific"
+        description: "Mode assignment for PINIO#1"
        min: 0
        max: 255
        type: uint8_t
      - name: pinio_box4
        field: permanentId[3]
+        default_value: "target specific"
+        description: "Mode assignment for PINIO#1"
        min: 0
        max: 255
        type: uint8_t
@ -3225,6 +3283,8 @@ groups:
    condition: USE_ESC_SENSOR
    members:
        - name: esc_sensor_listen_only
+          default_value: "OFF"
+          description: "Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case"
          field: listenOnly
          type: bool

@ -3261,3 +3321,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
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@ -104,6 +104,7 @@ typedef struct {
    uint16_t pidSumLimit;
    filterApply4FnPtr ptermFilterApplyFn;
    bool itermLimitActive;
+    bool itermFreezeActive;

    biquadFilter_t rateTargetFilter;
 } pidState_t;
@ -267,6 +268,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
        .fixedWingCoordinatedYawGain = 1.0f,
        .fixedWingCoordinatedPitchGain = 1.0f,
        .fixedWingItermLimitOnStickPosition = 0.5f,
+        .fixedWingYawItermBankFreeze = 0,

        .loiter_direction = NAV_LOITER_RIGHT,
        .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ,
@ -490,7 +492,7 @@ void updatePIDCoefficients()
            // Airplanes - scale all PIDs according to TPA
            pidState[axis].kP  = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER  * tpaFactor;
            pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER  * tpaFactor;
-            pidState[axis].kD  = 0.0f;
+            pidState[axis].kD  = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor;
            pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor;
            pidState[axis].kCD = 0.0f;
            pidState[axis].kT  = 0.0f;
@ -504,7 +506,7 @@ void updatePIDCoefficients()
            pidState[axis].kFF = 0.0f;

            // Tracking anti-windup requires P/I/D to be all defined which is only true for MC
-            if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0)) {
+            if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0) && (usedPidControllerType == PID_TYPE_PID)) {
                pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP));
            } else {
                pidState[axis].kT = 0;
@ -637,66 +639,6 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
    return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
 }

-static void applyItermLimiting(pidState_t *pidState) {
-    if (pidState->itermLimitActive) {
-        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
-    } else {
-        pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
-    }
-}
-
-static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
-    UNUSED(pidState);
-    UNUSED(axis);
-    UNUSED(dT);
-}
-
-static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
-{
-    const float rateError = pidState->rateTarget - pidState->gyroRate;
-    const float newPTerm = pTermProcess(pidState, rateError, dT);
-    const float newFFTerm = pidState->rateTarget * pidState->kFF;
-
-    // Calculate integral
-    pidState->errorGyroIf += rateError * pidState->kI * dT;
-
-    applyItermLimiting(pidState);
-
-    if (pidProfile()->fixedWingItermThrowLimit != 0) {
-        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
-    }
-
-#ifdef USE_AUTOTUNE_FIXED_WING
-    if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
-        autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm);
-    }
-#endif
-
-    axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit);
-
-#ifdef USE_BLACKBOX
-    axisPID_P[axis] = newPTerm;
-    axisPID_I[axis] = pidState->errorGyroIf;
-    axisPID_D[axis] = newFFTerm;
-    axisPID_Setpoint[axis] = pidState->rateTarget;
-#endif
-}
-
-static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
-{
-    if (itermRelax) {
-        if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
-
-            const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
-            const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
-            
-            const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
-            return itermErrorRate * itermRelaxFactor;
-        }
-    }
-
-    return itermErrorRate;
-}
 #ifdef USE_D_BOOST
 static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
    
@ -723,10 +665,100 @@ static float applyDBoost(pidState_t *pidState, float dT) {
 }
 #endif

+static float dTermProcess(pidState_t *pidState, float dT) {
+    // Calculate new D-term
+    float newDTerm = 0;
+    if (pidState->kD == 0) {
+        // optimisation for when D is zero, often used by YAW axis
+        newDTerm = 0;
+    } else {
+        float delta = pidState->previousRateGyro - pidState->gyroRate;
+
+        delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
+        delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
+
+        // Calculate derivative
+        newDTerm =  delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
+    }
+    return(newDTerm);
+}
+
+static void applyItermLimiting(pidState_t *pidState) {
+    if (pidState->itermLimitActive) {
+        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
+    } else 
+    {
+        pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
+    }
+}
+
+static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
+    UNUSED(pidState);
+    UNUSED(axis);
+    UNUSED(dT);
+}
+
+static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
+{
+    const float rateError = pidState->rateTarget - pidState->gyroRate;
+    const float newPTerm = pTermProcess(pidState, rateError, dT);
+    const float newDTerm = dTermProcess(pidState, dT);
+    const float newFFTerm = pidState->rateTarget * pidState->kFF;
+
+    DEBUG_SET(DEBUG_FW_D, axis, newDTerm);
+    /*
+     * Integral should be updated only if axis Iterm is not frozen
+     */
+    if (!pidState->itermFreezeActive) {
+        pidState->errorGyroIf += rateError * pidState->kI * dT;
+    }
+
+    applyItermLimiting(pidState);
+
+    if (pidProfile()->fixedWingItermThrowLimit != 0) {
+        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
+    }
+
+#ifdef USE_AUTOTUNE_FIXED_WING
+    if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
+        autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm);
+    }
+#endif
+
+    axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit);
+
+#ifdef USE_BLACKBOX
+    axisPID_P[axis] = newPTerm;
+    axisPID_I[axis] = pidState->errorGyroIf;
+    axisPID_D[axis] = newDTerm;
+    axisPID_Setpoint[axis] = pidState->rateTarget;
+#endif
+
+    pidState->previousRateGyro = pidState->gyroRate;
+
+}
+
+static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
+{
+    if (itermRelax) {
+        if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
+
+            const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
+            const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
+            
+            const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
+            return itermErrorRate * itermRelaxFactor;
+        }
+    }
+
+    return itermErrorRate;
+}
+
 static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 {
    const float rateError = pidState->rateTarget - pidState->gyroRate;
    const float newPTerm = pTermProcess(pidState, rateError, dT);
+    const float newDTerm = dTermProcess(pidState, dT);

    const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget;
    const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta);
@ -743,21 +775,6 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
    }
    DEBUG_SET(DEBUG_CD, axis, newCDTerm);

-    // Calculate new D-term
-    float newDTerm;
-    if (pidState->kD == 0) {
-        // optimisation for when D8 is zero, often used by YAW axis
-        newDTerm = 0;
-    } else {
-        float delta = pidState->previousRateGyro - pidState->gyroRate;
-
-        delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
-        delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
-
-        // Calculate derivative
-        newDTerm =  delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
-    }
-
    // TODO: Get feedback from mixer on available correction range for each axis
    const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
    const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
@ -890,7 +907,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, float pitchAngleTarget)
 {
    fpVector3_t targetRates;
    targetRates.x = 0.0f;
@ -918,8 +935,9 @@ 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;
+            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;

            targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
        }
@ -980,6 +998,24 @@ void checkItermLimitingActive(pidState_t *pidState)
    pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; 
 }

+void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
+{
+    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()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
+            pidState->itermFreezeActive = true;
+        } else
+        {
+            pidState->itermFreezeActive = false;
+        }
+    } else
+    {
+        pidState->itermFreezeActive = false;
+    }
+    
+}
+
 void FAST_CODE pidController(float dT)
 {
    if (!pidFiltersConfigured) {
@ -1032,8 +1068,10 @@ void FAST_CODE pidController(float dT)
        levelingEnabled = false;
    }

-    if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
-        pidTurnAssistant(pidState);
+    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]));
+        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
    }

@ -1050,6 +1088,8 @@ void FAST_CODE pidController(float dT)
        
        // Step 4: Run gyro-driven control
        checkItermLimitingActive(&pidState[axis]);
+        checkItermFreezingActive(&pidState[axis], axis);
+
        pidControllerApplyFn(&pidState[axis], axis, dT);
    }
 }
@ -1175,15 +1215,6 @@ pidBank_t * pidBankMutable(void) {
    return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }

-uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
-{
-    if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) {
-       return &pidBank->pid[pidIndex].FF;
-    } else {
-        return &pidBank->pid[pidIndex].D;
-    }
-}
-
 void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
 {
    if (!STATE(AIRPLANE)) {
@ -1240,4 +1271,4 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
    fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);

    previousArmingState = !!ARMING_FLAG(ARMED);
-}
+}
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@ -79,7 +79,7 @@ typedef enum {
 typedef enum {
    PID_TYPE_NONE = 0,  // Not used in the current platform/mixer/configuration
    PID_TYPE_PID,   // Uses P, I and D terms
-    PID_TYPE_PIFF,  // Uses P, I and FF, ignoring D
+    PID_TYPE_PIFF,  // Uses P, I, D and FF
    PID_TYPE_AUTO,  // Autodetect
 } pidType_e;

@ -131,6 +131,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    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;
@ -206,6 +207,5 @@ void autotuneUpdateState(void);
 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);

 pidType_e pidIndexGetType(pidIndex_e pidIndex);
-uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);

 void updateFixedWingLevelTrim(timeUs_t currentTimeUs);
--- a/src/main/flight/pid_autotune.c
+++ b/src/main/flight/pid_autotune.c
@ -230,15 +230,15 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa

            switch (axis) {
            case FD_ROLL:
-                blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D_FF, tuneCurrent[axis].gainFF);
+                blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF);
                break;

            case FD_PITCH:
-                blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D_FF, tuneCurrent[axis].gainFF);
+                blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF);
                break;

            case FD_YAW:
-                blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D_FF, tuneCurrent[axis].gainFF);
+                blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF);
                break;
            }
        }
--- a/src/main/io/lights.c
+++ b/src/main/io/lights.c
@ -72,7 +72,7 @@ void lightsUpdate(timeUs_t currentTimeUs)
        lightsSetStatus(IS_RC_MODE_ACTIVE(BOXLIGHTS), currentTimeUs);
 }

-void lightsInit()
+void lightsInit(void)
 {
    lightsHardwareInit();
 }
--- a/src/main/io/lights.h
+++ b/src/main/io/lights.h
@ -33,6 +33,6 @@ typedef struct lightsConfig_s {
 PG_DECLARE(lightsConfig_t, lightsConfig);

 void lightsUpdate(timeUs_t currentTimeUs);
-void lightsInit();
+void lightsInit(void);

 #endif /* USE_LIGHTS */
--- 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)
@ -712,6 +713,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;
@ -945,6 +948,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_pwm2centideg);
+}
+
 // Returns a heading angle in degrees normalized to [0, 360).
 int osdGetHeadingAngle(int angle)
 {
@ -1118,13 +1129,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) {
@ -1161,7 +1165,49 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_
    displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
 }

-static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
+static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD, adjustmentFunction_e adjFuncFF)
+{
+    textAttributes_t elemAttr;
+    char buff[4];
+
+    const pid8_t *pid = &pidBank()->pid[pidIndex];
+    pidType_e pidType = pidIndexGetType(pidIndex);
+
+    displayWrite(osdDisplayPort, elemPosX, elemPosY, str);
+
+    if (pidType == PID_TYPE_NONE) {
+        // PID is not used in this configuration. Draw dashes.
+        // XXX: Keep this in sync with the %3d format and spacing used below
+        displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "-   -   -   -");
+        return;
+    }
+
+    elemAttr = TEXT_ATTRIBUTES_NONE;
+    tfp_sprintf(buff, "%3d", pid->P);
+    if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P))))
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
+
+    elemAttr = TEXT_ATTRIBUTES_NONE;
+    tfp_sprintf(buff, "%3d", pid->I);
+    if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I))))
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr);
+
+    elemAttr = TEXT_ATTRIBUTES_NONE;
+    tfp_sprintf(buff, "%3d", pid->D);
+    if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
+    
+    elemAttr = TEXT_ATTRIBUTES_NONE;
+    tfp_sprintf(buff, "%3d", pid->FF);
+    if ((isAdjustmentFunctionSelected(adjFuncFF)) || (((adjFuncFF == ADJUSTMENT_ROLL_FF) || (adjFuncFF == ADJUSTMENT_PITCH_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_FF))))
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+    displayWriteWithAttr(osdDisplayPort, elemPosX + 16, elemPosY, buff, elemAttr);
+}
+
+static void osdDisplayNavPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
 {
    textAttributes_t elemAttr;
    char buff[4];
@ -1192,7 +1238,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *

    elemAttr = TEXT_ATTRIBUTES_NONE;
    tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D);
-    if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D_FF) || (adjFuncD == ADJUSTMENT_PITCH_D_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D_FF))))
+    if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
    displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
 }
@ -1334,7 +1380,11 @@ static bool osdDrawSingleElement(uint8_t item)
                }
                else
                {
-                    int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading());
+                    int16_t panHomeDirOffset = 0;
+                    if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
+                        panHomeDirOffset = osdPanServoHomeDirectionOffset();
+                    }
+                    int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset;
                    osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection);
                }
            } else {
@ -1854,35 +1904,35 @@ static bool osdDrawSingleElement(uint8_t item)
 #endif

    case OSD_ROLL_PIDS:
-        osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D_FF);
+        osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF);
        return true;

    case OSD_PITCH_PIDS:
-        osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D_FF);
+        osdDisplayFlightPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D, ADJUSTMENT_PITCH_FF);
        return true;

    case OSD_YAW_PIDS:
-        osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D_FF);
+        osdDisplayFlightPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D, ADJUSTMENT_YAW_FF);
        return true;

    case OSD_LEVEL_PIDS:
-        osdDisplayPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
+        osdDisplayNavPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
        return true;

    case OSD_POS_XY_PIDS:
-        osdDisplayPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
+        osdDisplayNavPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
        return true;

    case OSD_POS_Z_PIDS:
-        osdDisplayPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
+        osdDisplayNavPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
        return true;

    case OSD_VEL_XY_PIDS:
-        osdDisplayPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
+        osdDisplayNavPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
        return true;

    case OSD_VEL_Z_PIDS:
-        osdDisplayPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
+        osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
        return true;

    case OSD_HEADING_P:
@ -2186,12 +2236,21 @@ static bool osdDrawSingleElement(uint8_t item)
            }
            break;
        }
-
    case OSD_DEBUG:
        {
-            // Longest representable string is -2147483648, hence 11 characters
+            /* 
+             * Longest representable string is -2147483648 does not fit in the screen. 
+             * Only 7 digits for negative and 8 digits for positive values allowed
+             */
            for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) {
-                tfp_sprintf(buff, "[%u]=%11ld [%u]=%11ld", bufferIndex, debug[bufferIndex], bufferIndex+1, debug[bufferIndex+1]);
+                tfp_sprintf(
+                    buff, 
+                    "[%u]=%8ld [%u]=%8ld", 
+                    bufferIndex, 
+                    constrain(debug[bufferIndex], -9999999, 99999999), 
+                    bufferIndex+1, 
+                    constrain(debug[bufferIndex+1], -9999999, 99999999)
+                );
                displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
            }
            break;
@ -2590,6 +2649,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_pwm2centideg = 0,

    .units = OSD_UNIT_METRIC,
    .main_voltage_decimals = 1,
--- 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!"
@ -348,7 +349,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
+    int8_t pan_servo_pwm2centideg;      // Centidegrees of servo rotation per us pwm
    uint8_t crsf_lq_format;

 } osdConfig_t;
--- 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
@ -152,3 +154,12 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas)
 }

 #endif
+
+#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
--- 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
+
--- 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
@ -500,6 +503,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;
@ -639,13 +644,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)
@ -670,7 +668,7 @@ static void osdDJIFormatThrottlePosition(char *buff, bool autoThr )
        thr = rcCommand[THROTTLE];
    }

-    tfp_sprintf(buff, "%3d%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR");
+    tfp_sprintf(buff, "%3ld%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR");
 }

 /**
@ -766,7 +764,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 +1002,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;

--- 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);
--- 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
-
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@ -91,7 +91,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif

-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10);

 PG_RESET_TEMPLATE(navConfig_t, navConfig,
    .general = {
@ -144,6 +144,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
        .braking_bank_angle = 40,               // Max braking angle
        .posDecelerationTime = 120,             // posDecelerationTime * 100
        .posResponseExpo = 10,                  // posResponseExpo * 100
+        .slowDownForTurning = true,
    },

    // Fixed wing
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@ -219,6 +219,7 @@ typedef struct navConfig_s {
        uint8_t  braking_bank_angle;            // Max angle [deg] that MR is allowed duing braking boost phase
        uint8_t posDecelerationTime;            // Brake time parameter
        uint8_t posResponseExpo;                // Position controller expo (taret vel expo for MC)
+        bool slowDownForTurning;             // Slow down during WP missions when changing heading on next waypoint
    } mc;

    struct {
@ -250,7 +251,7 @@ typedef struct navConfig_s {
        uint8_t  launch_max_angle;           // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
        uint8_t  cruise_yaw_rate;            // Max yaw rate (dps) when CRUISE MODE is enabled
        bool     allow_manual_thr_increase;
-        bool useFwNavYawControl;
+        bool    useFwNavYawControl;
        uint8_t yawControlDeadband;
    } fw;
 } navConfig_t;
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@ -406,7 +406,7 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
 static float getVelocityHeadingAttenuationFactor(void)
 {
    // In WP mode scale velocity if heading is different from bearing
-    if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
+    if (navConfig()->mc.slowDownForTurning && (navGetCurrentStateFlags() & NAV_AUTO_WP)) {
        const int32_t headingError = constrain(wrap_18000(posControl.desiredState.yaw - posControl.actualState.yaw), -9000, 9000);
        const float velScaling = cos_approx(CENTIDEGREES_TO_RADIANS(headingError));

--- 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"
@ -317,6 +318,20 @@ 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;
+
+        case LOGIC_CONDITION_MODULUS:
+            if (operandB != 0) {
+                return constrain(operandA % operandB, INT16_MIN, INT16_MAX);
+            } else {
+                return operandA;
+            }
+            break;
+
        default:
            return false;
            break; 
@ -377,7 +392,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
            return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
            break;
        
-        case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 10
+        case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100
            return getBatteryVoltage();
            break;

@ -402,7 +417,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
@ -418,7 +433,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: // %
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@ -68,7 +68,9 @@ 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_MODULUS                     = 40,
+    LOGIC_CONDITION_LAST                        = 41,
 } logicOperation_e;

 typedef enum logicOperandType_s {
--- 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;
 }

--- 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);
+int32_t programmingPidGetOutput(uint8_t i);
--- a/src/main/rx/ghst_protocol.h
+++ b/src/main/rx/ghst_protocol.h
@ -65,6 +65,8 @@ typedef enum {
    GHST_DL_LINK_STAT           = 0x21,
    GHST_DL_VTX_STAT            = 0x22,
    GHST_DL_PACK_STAT           = 0x23,     // Battery (Pack) Status
+    GHST_DL_GPS_PRIMARY         = 0x25,     // Primary GPS data (position)
+    GHST_DL_GPS_SECONDARY       = 0x26      // Secondary GPS data (auxiliary)
 } ghstDl_e;

 #define GHST_RC_CTR_VAL_12BIT       0x7C0   // servo center for 12 bit values (0x3e0 << 1)
@ -72,9 +74,12 @@ typedef enum {

 #define GHST_FRAME_SIZE             14      // including addr, type, len, crc, and payload

-#define GHST_PAYLOAD_SIZE_MAX           14
+#define GHST_PAYLOAD_SIZE_MAX       14

-#define GHST_FRAME_SIZE_MAX             24
+#define GHST_FRAME_SIZE_MAX         24
+
+#define GPS_FLAGS_FIX               0x01
+#define GPS_FLAGS_FIX_HOME          0x02

 typedef struct ghstFrameDef_s {
    uint8_t addr;
--- 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)) {
--- 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;

--- 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;
--- 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"
@ -207,6 +208,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)) {
--- 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;

--- a/src/main/sensors/opflow.c
+++ b/src/main/sensors/opflow.c
@ -75,7 +75,7 @@ static float opflowCalibrationFlowAcc;
 #define OPFLOW_UPDATE_TIMEOUT_US        200000  // At least 5Hz updates required
 #define OPFLOW_CALIBRATE_TIME_MS        30000   // 30 second calibration time

-PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 2);

 PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig,
    .opflow_hardware = OPFLOW_NONE,
--- a/src/main/sensors/opflow.h
+++ b/src/main/sensors/opflow.h
@ -30,10 +30,9 @@

 typedef enum {
    OPFLOW_NONE         = 0,
-    OPFLOW_PMW3901      = 1,
-    OPFLOW_CXOF         = 2,
-    OPFLOW_MSP          = 3,
-    OPFLOW_FAKE         = 4,
+    OPFLOW_CXOF         = 1,
+    OPFLOW_MSP          = 2,
+    OPFLOW_FAKE         = 3,
 } opticalFlowSensor_e;

 typedef enum {
--- 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


--- a/src/main/telemetry/ghst.c
+++ b/src/main/telemetry/ghst.c
@ -43,6 +43,7 @@

 #include "drivers/nvic.h"

+#include "fc/config.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"

@ -51,6 +52,8 @@
 #include "io/gps.h"
 #include "io/serial.h"

+#include "navigation/navigation.h"
+
 #include "rx/rx.h"
 #include "rx/ghst.h"
 #include "rx/ghst_protocol.h"
@ -67,6 +70,7 @@

 #define GHST_CYCLETIME_US                   100000      // 10x/sec
 #define GHST_FRAME_PACK_PAYLOAD_SIZE        10
+#define GHST_FRAME_GPS_PAYLOAD_SIZE         10
 #define GHST_FRAME_LENGTH_CRC               1
 #define GHST_FRAME_LENGTH_TYPE              1

@ -112,11 +116,51 @@ void ghstFramePackTelemetry(sbuf_t *dst)
    sbufWriteU8(dst, 0x00);                     // tbd3
 }

+
+// GPS data, primary, positional data
+void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst)
+{
+    // use sbufWrite since CRC does not include frame length
+    sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
+    sbufWriteU8(dst, GHST_DL_GPS_PRIMARY);
+
+    sbufWriteU32(dst, gpsSol.llh.lat);
+    sbufWriteU32(dst, gpsSol.llh.lon);
+
+    // constrain alt. from -32,000m to +32,000m, units of meters
+    const int16_t altitude = (constrain(getEstimatedActualPosition(Z), -32000 * 100, 32000 * 100) / 100);
+    sbufWriteU16(dst, altitude);
+}
+
+// GPS data, secondary, auxiliary data
+void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
+{
+    // use sbufWrite since CRC does not include frame length
+    sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
+    sbufWriteU8(dst, GHST_DL_GPS_SECONDARY);
+
+    sbufWriteU16(dst, gpsSol.groundSpeed);      // speed in 0.1m/s
+    sbufWriteU16(dst, gpsSol.groundCourse);     // degrees * 10
+    sbufWriteU8(dst, gpsSol.numSat);
+
+    sbufWriteU16(dst, (uint16_t) (GPS_distanceToHome / 10));    // use units of 10m to increase range of U16 to 655.36km
+    sbufWriteU16(dst, GPS_directionToHome);
+
+    uint8_t gpsFlags = 0;
+    if(STATE(GPS_FIX))
+        gpsFlags |= GPS_FLAGS_FIX;
+    if(STATE(GPS_FIX_HOME))
+        gpsFlags |= GPS_FLAGS_FIX_HOME;
+    sbufWriteU8(dst, gpsFlags);
+}
+
 // schedule array to decide how often each type of frame is sent
 typedef enum {
    GHST_FRAME_START_INDEX = 0,
    GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data
-    GHST_SCHEDULE_COUNT_MAX
+    GHST_FRAME_GPS_PRIMARY_INDEX,                   // GPS, primary values (Lat, Long, Alt)
+    GHST_FRAME_GPS_SECONDARY_INDEX,                 // GPS, secondary values (Sat Count, HDOP, etc.)
+   GHST_SCHEDULE_COUNT_MAX
 } ghstFrameTypeIndex_e;

 static uint8_t ghstScheduleCount;
@ -136,6 +180,19 @@ static void processGhst(void)
        ghstFramePackTelemetry(dst);
        ghstFinalize(dst);
    } 
+
+    if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) {
+        ghstInitializeFrame(dst);
+        ghstFrameGpsPrimaryTelemetry(dst);
+        ghstFinalize(dst);
+    }
+
+   if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) {
+        ghstInitializeFrame(dst);
+        ghstFrameGpsSecondaryTelemetry(dst);
+        ghstFinalize(dst);
+    }
+
    ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount;
 }

@ -152,6 +209,14 @@ void initGhstTelemetry(void)
    if (isBatteryVoltageConfigured() || isAmperageConfigured()) {
        ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX);
    }
+
+#ifdef USE_GPS
+    if (feature(FEATURE_GPS)) {
+        ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX);
+        ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX);
+    }
+#endif
+
    ghstScheduleCount = (uint8_t)index;
 }