Merge branch 'master' into RTH-Altitude-Override

2025-07-20 06:45:14 +03:00 · 2021-02-10 23:39:42 +00:00 · 2021-02-10 23:39:42 +00:00 · edc98ab6c1
commit edc98ab6c1
parent 0a93b8b64a 881e6806e7
88 changed files with 1493 additions and 1338 deletions
--- a/.github/workflows/docs.yml
+++ b/.github/workflows/docs.yml
@ -1,5 +1,9 @@
 name: Make sure docs are updated
 on:
+  pull_request:
+    paths:
+      - src/main/fc/settings.yaml
+      - docs/Settings.md
  push:
    paths:
      - src/main/fc/settings.yaml
--- a/cmake/stm32f7.cmake
+++ b/cmake/stm32f7.cmake
@ -97,8 +97,6 @@ function(target_stm32f7xx)
        VCP_SOURCES ${STM32F7_USB_SRC} ${STM32F7_VCP_SRC}
        VCP_INCLUDE_DIRECTORIES ${STM32F7_USB_INCLUDE_DIRS} ${STM32F7_VCP_DIR}

-        OPTIMIZATION -O2
-
        OPENOCD_TARGET stm32f7x

        BOOTLOADER
@ -112,6 +110,11 @@ macro(define_target_stm32f7 subfamily size)
        set(func_ARGV ARGV)
        string(TOUPPER ${size} upper_size)
        get_stm32_flash_size(flash_size ${size})
+        if(flash_size GREATER 512)
+            set(opt -O2)
+        else()
+            set(opt -Os)
+        endif()
        set(definitions
            STM32F7
            STM32F7${subfamily}xx
@ -123,6 +126,8 @@ macro(define_target_stm32f7 subfamily size)
            STARTUP startup_stm32f7${subfamily}xx.s
            COMPILE_DEFINITIONS ${definitions}
            LINKER_SCRIPT stm32_flash_f7${subfamily}x${size}
+            OPTIMIZATION ${opt}
+
            ${${func_ARGV}}
        )
    endfunction()
--- a/docs/Board
+++ b/docs/Board
@ -79,6 +79,11 @@ More target options:
 * SBUS inverter connected to UART1
 * Uses target **FIREWORKSV2**

+### Omnibus Corner Nano
+
+* Configurable inverter on UART6
+* Use target **OMNIBUSF4V3**
+
 ## **NOT** supported

 * HC-SR04 Rangefinder
--- a/docs/Buzzer.md
+++ b/docs/Buzzer.md
@ -43,6 +43,8 @@ Sequences:
    14   DISARM_REPEAT         0, 100, 10		Stick held in disarm position (after pause)
    15   ARMED                 0, 245, 10, 5	Board is armed (after pause ; repeats until board is disarmed or throttle is increased)

+You can use [this tool](https://www.mrd-rc.com/tutorials-tools-and-testing/useful-tools/helpful-inav-buzzer-code-checker/) to hear current buzzer sequences or enter custom sequences.
+
 ## Controlling buzzer usage

 The usage of the buzzer can be controlled by the CLI `beeper` command.
--- a/docs/Programming
+++ b/docs/Programming
@ -9,10 +9,13 @@ INAV Programming Framework coinsists of:

 * Logic Conditions - each Logic Condition can be understood as a single command, a single line of code
 * Global Variables - variables that can store values from and for LogiC Conditions and servo mixer
+* Programming PID - general purpose, user configurable PID controllers

 IPF can be edited using INAV Configurator user interface, of via CLI

-## CLI
+## Logic Conditions
+
+### CLI

 `logic <rule> <enabled> <activatorId> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>`

@ -80,6 +83,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 3             | FLIGHT_MODE   | `value` points to flight modes table                  |
 | 4             | LC            | `value` points to other logic condition ID            |
 | 5             | GVAR          | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
+| 5             | PID          | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |

 #### FLIGHT

@ -120,7 +124,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 32            | CROSSFIRE LQ          | Crossfire Link quality as returned by the CRSF protocol   | 
 | 33            | CROSSFIRE SNR          | Crossfire SNR as returned by the CRSF protocol   | 

-##### ACTIVE_WAYPOINT_ACTION
+#### ACTIVE_WAYPOINT_ACTION

 | Action        |  Value   |
 |----           |----      |
@ -158,6 +162,27 @@ All flags are reseted on ARM and DISARM event.
 |----   |----       |----                   |
 | 0     | 1         | Latch - after activation LC will stay active until LATCH flag is reseted |

+## Global variables
+
+### CLI
+
+`gvar <index> <default value> <min> <max>`
+
+## Programming PID
+
+`pid <index> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>`
+
+* `<index>` - ID of PID Controller, starting from `0`
+* `<enabled>` - `0` evaluates as disabled, `1` evaluates as enabled
+* `<setpoint type>` - See `Operands` paragraph
+* `<setpoint value>` - See `Operands` paragraph
+* `<measurement type>` - See `Operands` paragraph
+* `<measurement value>` - See `Operands` paragraph
+* `<P gain>` - P-gain, scaled to `1/1000`
+* `<I gain>` - I-gain, scaled to `1/1000`
+* `<D gain>` - D-gain, scaled to `1/1000`
+* `<FF gain>` - FF-gain, scaled to `1/1000`
+
 ## Examples

 ### Dynamic THROTTLE scale
--- a/docs/Safehomes.md
+++ b/docs/Safehomes.md
@ -53,15 +53,17 @@ Parameters:
  * `<lat>` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789).
  * `<lon>` - Longitude.

+Note that coordinates from Google Maps only have five or six decimals, so you need to pad zero decimals until you have seven before removing the decimal period to set the correct safehome location. For example, coordinates 54.353319 -4.517927 obtained from Google Maps need to be entered as 543533190 -45179270, coordiniates 43.54648 -7.86545 as 435464800 -78654500 and 51.309842 -0.095651 as 513098420 -00956510.
+
 Safehomes are saved along with your regular settings and will appear in `diff` and `dump` output.  Use `save` to save any changes, as with other settings. 

 ### `safehome` example

 ```
 # safehome
-safehome 0 1 543533193 -45179273
-safehome 1 1 435464846 -78654544
-safehome 2 0 0 0 
+safehome 0 1 543533190 -45179270
+safehome 1 1 435464800 -78654500
+safehome 2 1 513098420 -00956510
 safehome 3 0 0 0 
 safehome 4 0 0 0 
 safehome 5 0 0 0 
--- a/docs/Settings.md
+++ b/docs/Settings.md
@ -20,6 +20,8 @@
 | acczero_x | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
 | acczero_y | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
 | acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
+| airmode_throttle_threshold | 1300 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used |
+| airmode_type | STICK_CENTER | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. |
 | airspeed_adc_channel | 0 | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 |
 | align_acc | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. |
 | align_board_pitch | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc |
@ -120,6 +122,7 @@
 | fw_i_yaw | 10 | Fixed-wing rate stabilisation I-gain for YAW |
 | fw_iterm_limit_stick_position | 0.5 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side |
 | fw_iterm_throw_limit | 165 | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. |
+| fw_level_pitch_trim | 0 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level |
 | fw_loiter_direction | RIGHT | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. |
 | fw_min_throttle_down_pitch | 0 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) |
 | fw_p_level | 20 | Fixed-wing attitude stabilisation P-gain |
@ -137,6 +140,9 @@
 | gps_provider | UBLOX | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). |
 | gps_sbas_mode | NONE | Which SBAS mode to be used |
 | gps_ublox_use_galileo | OFF | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. |
+| 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_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. |
@ -146,6 +152,7 @@
 | gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
 | gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf |
 | gyro_to_use |  |  |
+| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. |
 | has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot |
 | heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
 | hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry |
@ -212,8 +219,6 @@
 | max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° |
 | max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. |
 | max_throttle | 1850 | This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000. |
-| mc_airmode_threshold | 1300 | Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used |
-| mc_airmode_type | STICK_CENTER | Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode. |
 | mc_cd_lpf_hz | 30 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky |
 | mc_cd_pitch | 60 | Multicopter Control Derivative gain for PITCH |
 | mc_cd_roll | 60 | Multicopter Control Derivative gain for ROLL |
@ -273,7 +278,7 @@
 | nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) |
 | nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) |
 | nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection [cm/s] |
-| nav_fw_loiter_radius | 5000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] |
+| nav_fw_loiter_radius | 7500 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] |
 | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes |
 | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes |
 | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) |
@ -324,7 +329,7 @@
 | 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_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 OFF_ALWAYS 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_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) |
 | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
 | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. |
@ -368,6 +373,7 @@
 | osd_gforce_alarm | 5 | Value above which the OSD g force indicator will blink (g) |
 | osd_gforce_axis_alarm_max | 5 | Value above which the OSD axis g force indicators will blink (g) |
 | osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) |
+| osd_home_position_arm_screen | ON | Should home position coordinates be displayed on the arming screen. |
 | osd_horizon_offset | 0 | To vertically adjust the whole OSD and AHI and scrolling bars |
 | osd_hud_homepoint | 0 | To 3D-display the home point location in the hud |
 | osd_hud_homing | 0 | To display little arrows around the crossair showing where the home point is in the hud |
@ -385,7 +391,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_plus_code_digits |  |  |
+| 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 |  |  |
 | osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar |
 | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) |
@ -496,6 +503,7 @@
 | vtx_max_power_override | 0 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities |
 | vtx_pit_mode_chan |  |  |
 | vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. |
+| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. |
 | yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. |
 | yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
 | yaw_rate | 20 | Defines rotation rate on YAW 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. |
--- a/docs/development/Building
+++ b/docs/development/Building
@ -69,7 +69,7 @@ For 2.6 and later, inav uses `cmake` as its primary build tool. `cmake` simplies

 ## Using `cmake`

-The canonanical method of using `cmake` is to create a `build` directory and run the `cmake` and `make` commands from within the `build` directory. So, assuming we've cloned the firmware repository into an `inav` directory, we can issue the following commands to set up the build environment.
+The canonical method of using `cmake` is to create a `build` directory and run the `cmake` and `make` commands from within the `build` directory. So, assuming we've cloned the firmware repository into an `inav` directory, we can issue the following commands to set up the build environment.

 ```
 cd inav
--- a/docs/development/Cmake
+++ b/docs/development/Cmake
@ -67,3 +67,7 @@ target_stm32f405xg(OMNIBUSF4V3_S6_SS)
 # except for an inverter on UART6.
 target_stm32f405xg(OMNIBUSF4V3)
 ```
+
+## Adding (or removing) a source file
+
+In the cmake system, project source files are listed in `src/main/CMakeLists.txt`. New source files must be added to this list to be considered by the build system.
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@ -24,6 +24,8 @@ main_sources(COMMON_SRC
    common/encoding.h
    common/filter.c
    common/filter.h
+    common/fp_pid.c
+    common/fp_pid.h
    common/gps_conversion.c
    common/gps_conversion.h
    common/log.c
@ -321,6 +323,8 @@ main_sources(COMMON_SRC
    flight/rpm_filter.h
    flight/dynamic_gyro_notch.c
    flight/dynamic_gyro_notch.h
+    flight/dynamic_lpf.c
+    flight/dynamic_lpf.h

    io/beeper.c
    io/beeper.h
@ -362,6 +366,8 @@ main_sources(COMMON_SRC
    programming/global_variables.h
    programming/programming_task.c
    programming/programming_task.h
+    programming/pid.c
+    programming/pid.h

    rx/crsf.c
    rx/crsf.h
@ -402,8 +408,6 @@ main_sources(COMMON_SRC
    rx/sumd.h
    rx/sumh.c
    rx/sumh.h
-    rx/uib_rx.c
-    rx/uib_rx.h
    rx/xbus.c
    rx/xbus.h

@ -431,10 +435,6 @@ main_sources(COMMON_SRC
    sensors/temperature.c
    sensors/temperature.h

-    uav_interconnect/uav_interconnect.h
-    uav_interconnect/uav_interconnect_bus.c
-    uav_interconnect/uav_interconnect_rangefinder.c
-
    blackbox/blackbox.c
    blackbox/blackbox.h
    blackbox/blackbox_encoding.c
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@ -79,5 +79,6 @@ typedef enum {
    DEBUG_SPM_VS600,    // Smartport master VS600 VTX
    DEBUG_SPM_VARIO,    // Smartport master variometer
    DEBUG_PCF8574,
+    DEBUG_DYNAMIC_GYRO_LPF,
    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_UINT8_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
+#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_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/filter.c
+++ b/src/main/common/filter.c
@ -28,9 +28,6 @@ FILE_COMPILE_FOR_SPEED
 #include "common/maths.h"
 #include "common/utils.h"

-#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
-#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
-
 // NULL filter
 float nullFilterApply(void *filter, float input)
 {
@ -48,17 +45,23 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt)

 // PT1 Low Pass filter

+static float pt1ComputeRC(const float f_cut)
+{
+    return 1.0f / (2.0f * M_PIf * f_cut);
+}
+
 // f_cut = cutoff frequency
 void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
 {
    filter->state = 0.0f;
    filter->RC = tau;
    filter->dT = dT;
+    filter->alpha = filter->dT / (filter->RC + filter->dT);
 }

 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
 {
-    pt1FilterInitRC(filter, 1.0f / (2.0f * M_PIf * f_cut), dT);
+    pt1FilterInitRC(filter, pt1ComputeRC(f_cut), dT);
 }

 void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
@ -69,9 +72,15 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) {
    return filter->state;
 }

+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut)
+{
+    filter->RC = pt1ComputeRC(f_cut);
+    filter->alpha = filter->dT / (filter->RC + filter->dT);
+}
+
 float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
 {
-    filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
+    filter->state = filter->state + filter->alpha * (input - filter->state);
    return filter->state;
 }

@ -86,11 +95,12 @@ float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float
 {
    // Pre calculate and store RC
    if (!filter->RC) {
-        filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut );
+        filter->RC = pt1ComputeRC(f_cut);
    }

    filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
-    filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
+    filter->alpha = filter->dT / (filter->RC + filter->dT);
+    filter->state = filter->state + filter->alpha * (input - filter->state);
    return filter->state;
 }

--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@ -25,6 +25,7 @@ typedef struct pt1Filter_s {
    float state;
    float RC;
    float dT;
+    float alpha;
 } pt1Filter_t;

 /* this holds the data required to update samples thru a filter */
@ -58,12 +59,16 @@ typedef struct firFilter_s {
 typedef float (*filterApplyFnPtr)(void *filter, float input);
 typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);

+#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
+#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
+
 float nullFilterApply(void *filter, float input);
 float nullFilterApply4(void *filter, float input, float f_cut, float dt);

 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT);
 void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT);
 void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut);
 float pt1FilterGetLastOutput(pt1Filter_t *filter);
 float pt1FilterApply(pt1Filter_t *filter, float input);
 float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
--- a/src/main/common/fp_pid.c
+++ b/src/main/common/fp_pid.c
@ -0,0 +1,156 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "platform.h"
+
+FILE_COMPILE_FOR_SPEED
+
+#include <math.h>
+#include "common/fp_pid.h"
+
+/*-----------------------------------------------------------
+ * Float point PID-controller implementation
+ *-----------------------------------------------------------*/
+// Implementation of PID with back-calculation I-term anti-windup
+// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
+// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
+float navPidApply3(
+    pidController_t *pid,
+    const float setpoint,
+    const float measurement,
+    const float dt,
+    const float outMin,
+    const float outMax,
+    const pidControllerFlags_e pidFlags,
+    const float gainScaler,
+    const float dTermScaler
+) {
+    float newProportional, newDerivative, newFeedForward;
+    float error = setpoint - measurement;
+
+    /* P-term */
+    newProportional = error * pid->param.kP * gainScaler;
+
+    /* D-term */
+    if (pid->reset) {
+        pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement;
+        pid->reset = false;
+    }
+
+    if (pidFlags & PID_DTERM_FROM_ERROR) {
+        /* Error-tracking D-term */
+        newDerivative = (error - pid->last_input) / dt;
+        pid->last_input = error;
+    } else {
+        /* Measurement tracking D-term */
+        newDerivative = -(measurement - pid->last_input) / dt;
+        pid->last_input = measurement;
+    }
+
+    if (pid->dTermLpfHz > 0) {
+        newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
+    } else {
+        newDerivative = pid->param.kD * newDerivative;
+    }
+
+    newDerivative = newDerivative * gainScaler * dTermScaler;
+
+    if (pidFlags & PID_ZERO_INTEGRATOR) {
+        pid->integrator = 0.0f;
+    }
+
+    /*
+     * Compute FeedForward parameter
+     */
+    newFeedForward = setpoint * pid->param.kFF * gainScaler;
+
+    /* Pre-calculate output and limit it if actuator is saturating */
+    const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
+    const float outValConstrained = constrainf(outVal, outMin, outMax);
+
+    pid->proportional = newProportional;
+    pid->integral = pid->integrator;
+    pid->derivative = newDerivative;
+    pid->feedForward = newFeedForward;
+    pid->output_constrained = outValConstrained;
+
+    /* Update I-term */
+    if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
+        const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
+
+        if (pidFlags & PID_SHRINK_INTEGRATOR) {
+            // Only allow integrator to shrink
+            if (fabsf(newIntegrator) < fabsf(pid->integrator)) {
+                pid->integrator = newIntegrator;
+            }
+        }
+        else {
+            pid->integrator = newIntegrator;
+        }
+    }
+    
+    if (pidFlags & PID_LIMIT_INTEGRATOR) {
+        pid->integrator = constrainf(pid->integrator, outMin, outMax);
+    } 
+
+    return outValConstrained;
+}
+
+float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
+{
+    return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f);
+}
+
+void navPidReset(pidController_t *pid)
+{
+    pid->reset = true;
+    pid->proportional = 0.0f;
+    pid->integral = 0.0f;
+    pid->derivative = 0.0f;
+    pid->integrator = 0.0f;
+    pid->last_input = 0.0f;
+    pid->feedForward = 0.0f;
+    pt1FilterReset(&pid->dterm_filter_state, 0.0f);
+    pid->output_constrained = 0.0f;
+}
+
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz)
+{
+    pid->param.kP = _kP;
+    pid->param.kI = _kI;
+    pid->param.kD = _kD;
+    pid->param.kFF = _kFF;
+
+    if (_kI > 1e-6f && _kP > 1e-6f) {
+        float Ti = _kP / _kI;
+        float Td = _kD / _kP;
+        pid->param.kT = 2.0f / (Ti + Td);
+    }
+    else {
+        pid->param.kI = 0.0;
+        pid->param.kT = 0.0;
+    }
+    pid->dTermLpfHz = _dTermLpfHz;
+    navPidReset(pid);
+}
--- a/src/main/common/fp_pid.h
+++ b/src/main/common/fp_pid.h
@ -0,0 +1,75 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+
+#include "common/filter.h"
+#include "common/maths.h"
+
+typedef struct {
+    float kP;
+    float kI;
+    float kD;
+    float kT;   // Tracking gain (anti-windup)
+    float kFF;  // FeedForward Component
+} pidControllerParam_t;
+
+typedef enum {
+    PID_DTERM_FROM_ERROR            = 1 << 0,
+    PID_ZERO_INTEGRATOR             = 1 << 1,
+    PID_SHRINK_INTEGRATOR           = 1 << 2,
+    PID_LIMIT_INTEGRATOR            = 1 << 3,
+} pidControllerFlags_e;
+
+typedef struct {
+    bool reset;
+    pidControllerParam_t param;
+    pt1Filter_t dterm_filter_state;     // last derivative for low-pass filter
+    float dTermLpfHz;                   // dTerm low pass filter cutoff frequency
+    float integrator;                   // integrator value
+    float last_input;                   // last input for derivative
+
+    float integral;                     // used integral value in output
+    float proportional;                 // used proportional value in output
+    float derivative;                   // used derivative value in output
+    float feedForward;                  // used FeedForward value in output
+    float output_constrained;           // controller output constrained
+} pidController_t;
+
+float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
+float navPidApply3(
+    pidController_t *pid,
+    const float setpoint,
+    const float measurement,
+    const float dt,
+    const float outMin,
+    const float outMax,
+    const pidControllerFlags_e pidFlags,
+    const float gainScaler,
+    const float dTermScaler
+);
+void navPidReset(pidController_t *pid);
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@ -118,8 +118,13 @@ bool isEEPROMContentValid(void)
            // Found the end.  Stop scanning.
            break;
        }
-        if (p + record->size >= &__config_end
-            || record->size < sizeof(*record)) {
+
+        if (p + sizeof(*record) >= &__config_end) {
+            // Too big. Further checking for size doesn't make sense
+            return false;
+        }
+
+        if (p + record->size >= &__config_end || record->size < sizeof(*record)) {
            // Too big or too small.
            return false;
        }
@ -152,13 +157,21 @@ static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFla
    p += sizeof(configHeader_t);             // skip header
    while (true) {
        const configRecord_t *record = (const configRecord_t *)p;
-        if (record->size == 0
-            || p + record->size >= &__config_end
-            || record->size < sizeof(*record))
+        // Ensure that the record header fits into config memory, otherwise accessing size and flags may cause a hardfault.
+        if (p + sizeof(*record) >= &__config_end) {
            break;
-        if (pgN(reg) == record->pgn
-            && (record->flags & CR_CLASSIFICATION_MASK) == classification)
+        }
+
+        // Check that record header makes sense
+        if (record->size == 0 || p + record->size >= &__config_end || record->size < sizeof(*record)) {
+            break;
+        }
+
+        // Check if this is the record we're looking for (check for size)
+        if (pgN(reg) == record->pgn && (record->flags & CR_CLASSIFICATION_MASK) == classification) {
            return record;
+        }
+
        p += record->size;
    }
    // record not found
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@ -115,7 +115,8 @@
 #define PG_OSD_LAYOUTS_CONFIG 1025
 #define PG_SAFE_HOME_CONFIG 1026
 #define PG_DJI_OSD_CONFIG 1027
-#define PG_INAV_END 1027
+#define PG_PROGRAMMING_PID 1028
+#define PG_INAV_END 1028

 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
--- a/src/main/drivers/opflow/opflow_virtual.c
+++ b/src/main/drivers/opflow/opflow_virtual.c
@ -23,7 +23,7 @@
 */

 /*
- * This is a bridge driver between a sensor reader that operates at high level (on top of UART or UIB)
+ * This is a bridge driver between a sensor reader that operates at high level (i.e. on top of UART)
 */

 #include <stdbool.h>
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@ -268,7 +268,8 @@ static bool motorsUseHardwareTimers(void)

 static bool servosUseHardwareTimers(void)
 {
-    return servoConfig()->servo_protocol == SERVO_TYPE_PWM;
+    return servoConfig()->servo_protocol == SERVO_TYPE_PWM ||
+        servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM;
 }

 static void pwmInitMotors(timMotorServoHardware_t * timOutputs)
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@ -52,7 +52,8 @@ typedef enum {
 typedef enum {
    SERVO_TYPE_PWM = 0,
    SERVO_TYPE_SERVO_DRIVER,
-    SERVO_TYPE_SBUS
+    SERVO_TYPE_SBUS,
+    SERVO_TYPE_SBUS_PWM
 } servoProtocolType_e;

 typedef enum {
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@ -501,6 +501,14 @@ static void pwmServoWriteStandard(uint8_t index, uint16_t value)
    }
 }

+#ifdef USE_SERVO_SBUS
+static void sbusPwmWriteStandard(uint8_t index, uint16_t value)
+{
+    pwmServoWriteStandard(index, value);
+    sbusServoUpdate(index, value);
+}
+#endif
+
 #ifdef USE_PWM_SERVO_DRIVER
 static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value)
 {
@ -532,6 +540,11 @@ void pwmServoPreconfigure(void)
            sbusServoInitialize();
            servoWritePtr = sbusServoUpdate;
            break;
+
+        case SERVO_TYPE_SBUS_PWM:
+            sbusServoInitialize();
+            servoWritePtr = sbusPwmWriteStandard;
+            break;
 #endif
    }
 }
--- a/src/main/drivers/rangefinder/rangefinder_virtual.c
+++ b/src/main/drivers/rangefinder/rangefinder_virtual.c
@ -23,7 +23,7 @@
 */

 /*
- * This is a bridge driver between a sensor reader that operates at high level (on top of UART or UIB)
+ * This is a bridge driver between a sensor reader that operates at high level (i.e. on top of UART)
 */

 #include <stdbool.h>
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@ -41,7 +41,9 @@ uint8_t cliMode = 0;
 #include "common/memory.h"
 #include "common/time.h"
 #include "common/typeconversion.h"
+#include "common/fp_pid.h"
 #include "programming/global_variables.h"
+#include "programming/pid.h"

 #include "config/config_eeprom.h"
 #include "config/feature.h"
@ -1207,10 +1209,10 @@ static void cliTempSensor(char *cmdline)
    } else {
        int16_t i;
        const char *ptr = cmdline, *label;
-        int16_t type, alarm_min, alarm_max;
+        int16_t type=0, alarm_min=0, alarm_max=0;
        bool addressValid = false;
        uint64_t address;
-        int8_t osdSymbol;
+        int8_t osdSymbol=0;
        uint8_t validArgumentCount = 0;
        i = fastA2I(ptr);
        if (i >= 0 && i < MAX_TEMP_SENSORS) {
@ -1299,8 +1301,8 @@ static void cliSafeHomes(char *cmdline)
    } else if (sl_strcasecmp(cmdline, "reset") == 0) {
        resetSafeHomes();
    } else {
-        int32_t lat, lon;
-        bool enabled;
+        int32_t lat=0, lon=0;
+        bool enabled=false;
        uint8_t validArgumentCount = 0;
        const char *ptr = cmdline;
        int8_t i = fastA2I(ptr);
@ -1401,9 +1403,9 @@ static void cliWaypoints(char *cmdline)
            cliShowParseError();
        }
    } else {
-        int16_t i, p1,p2=0,p3=0,tmp;
-        uint8_t action, flag;
-        int32_t lat, lon, alt;
+        int16_t i, p1=0,p2=0,p3=0,tmp=0;
+        uint8_t action=0, flag=0;
+        int32_t lat=0, lon=0, alt=0;
        uint8_t validArgumentCount = 0;
        const char *ptr = cmdline;
        i = fastA2I(ptr);
@ -1457,7 +1459,7 @@ static void cliWaypoints(char *cmdline)

            if (!(validArgumentCount == 6 || validArgumentCount == 8)) {
                cliShowParseError();
-            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
+            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
                cliShowParseError();
            } else {
                posControl.waypointList[i].action = action;
@ -1998,6 +2000,118 @@ static void cliGvar(char *cmdline) {
    }
 }

+static void printPid(uint8_t dumpMask, const programmingPid_t *programmingPids, const programmingPid_t *defaultProgrammingPids)
+{
+    const char *format = "pid %d %d %d %d %d %d %d %d %d %d";
+    for (uint32_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        const programmingPid_t pid = programmingPids[i];
+
+        bool equalsDefault = false;
+        if (defaultProgrammingPids) {
+            programmingPid_t defaultValue = defaultProgrammingPids[i];
+            equalsDefault =
+                pid.enabled == defaultValue.enabled &&
+                pid.setpoint.type == defaultValue.setpoint.type &&
+                pid.setpoint.value == defaultValue.setpoint.value &&
+                pid.measurement.type == defaultValue.measurement.type &&
+                pid.measurement.value == defaultValue.measurement.value &&
+                pid.gains.P == defaultValue.gains.P &&
+                pid.gains.I == defaultValue.gains.I &&
+                pid.gains.D == defaultValue.gains.D &&
+                pid.gains.FF == defaultValue.gains.FF;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format,
+                i,
+                pid.enabled,
+                pid.setpoint.type,
+                pid.setpoint.value,
+                pid.measurement.type,
+                pid.measurement.value,
+                pid.gains.P,
+                pid.gains.I,
+                pid.gains.D,
+                pid.gains.FF
+            );
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format,
+            i,
+            pid.enabled,
+            pid.setpoint.type,
+            pid.setpoint.value,
+            pid.measurement.type,
+            pid.measurement.value,
+            pid.gains.P,
+            pid.gains.I,
+            pid.gains.D,
+            pid.gains.FF
+        );
+    }
+}
+
+static void cliPid(char *cmdline) {
+    char * saveptr;
+    int args[10], check = 0;
+    uint8_t len = strlen(cmdline);
+
+    if (len == 0) {
+        printPid(DUMP_MASTER, programmingPids(0), NULL);
+    } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
+        pgResetCopy(programmingPidsMutable(0), PG_LOGIC_CONDITIONS);
+    } else {
+        enum {
+            INDEX = 0,
+            ENABLED,
+            SETPOINT_TYPE,
+            SETPOINT_VALUE,
+            MEASUREMENT_TYPE,
+            MEASUREMENT_VALUE,
+            P_GAIN,
+            I_GAIN,
+            D_GAIN,
+            FF_GAIN,
+            ARGS_COUNT
+            };
+        char *ptr = strtok_r(cmdline, " ", &saveptr);
+        while (ptr != NULL && check < ARGS_COUNT) {
+            args[check++] = fastA2I(ptr);
+            ptr = strtok_r(NULL, " ", &saveptr);
+        }
+
+        if (ptr != NULL || check != ARGS_COUNT) {
+            cliShowParseError();
+            return;
+        }
+
+        int32_t i = args[INDEX];
+        if (
+            i >= 0 && i < MAX_PROGRAMMING_PID_COUNT &&
+            args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
+            args[SETPOINT_TYPE] >= 0 && args[SETPOINT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[SETPOINT_VALUE] >= -1000000 && args[SETPOINT_VALUE] <= 1000000 &&
+            args[MEASUREMENT_TYPE] >= 0 && args[MEASUREMENT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[MEASUREMENT_VALUE] >= -1000000 && args[MEASUREMENT_VALUE] <= 1000000 &&
+            args[P_GAIN] >= 0 && args[P_GAIN] <= INT16_MAX &&
+            args[I_GAIN] >= 0 && args[I_GAIN] <= INT16_MAX &&
+            args[D_GAIN] >= 0 && args[D_GAIN] <= INT16_MAX &&
+            args[FF_GAIN] >= 0 && args[FF_GAIN] <= INT16_MAX
+        ) {
+            programmingPidsMutable(i)->enabled = args[ENABLED];
+            programmingPidsMutable(i)->setpoint.type = args[SETPOINT_TYPE];
+            programmingPidsMutable(i)->setpoint.value = args[SETPOINT_VALUE];
+            programmingPidsMutable(i)->measurement.type = args[MEASUREMENT_TYPE];
+            programmingPidsMutable(i)->measurement.value = args[MEASUREMENT_VALUE];
+            programmingPidsMutable(i)->gains.P = args[P_GAIN];
+            programmingPidsMutable(i)->gains.I = args[I_GAIN];
+            programmingPidsMutable(i)->gains.D = args[D_GAIN];
+            programmingPidsMutable(i)->gains.FF = args[FF_GAIN];
+
+            cliPid("");
+        } else {
+            cliShowParseError();
+        }
+    }
+}
+
 #endif

 #ifdef USE_SDCARD
@ -3312,6 +3426,9 @@ static void printConfig(const char *cmdline, bool doDiff)

        cliPrintHashLine("gvar");
        printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
+
+        cliPrintHashLine("pid");
+        printPid(dumpMask, programmingPids_CopyArray, programmingPids(0));
 #endif

        cliPrintHashLine("feature");
@ -3566,6 +3683,10 @@ const clicmd_t cmdTable[] = {
    CLI_COMMAND_DEF("gvar", "configure global variables",
        "<gvar> <default> <min> <max>\r\n"
        "\treset\r\n", cliGvar),
+
+    CLI_COMMAND_DEF("pid", "configurable PID controllers",
+        "<#> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>\r\n"
+        "\treset\r\n", cliPid),
 #endif
    CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
    CLI_COMMAND_DEF("smix", "servo mixer",
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@ -217,7 +217,7 @@ void validateAndFixConfig(void)
 #endif

 #ifndef USE_SERVO_SBUS
-    if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) {
+    if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS || servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM) {
        servoConfigMutable()->servo_protocol = SERVO_TYPE_PWM;
    }
 #endif
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@ -89,6 +89,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/failsafe.h"

 #include "config/feature.h"
+#include "programming/pid.h"

 // June 2013     V2.2-dev

@ -390,6 +391,7 @@ void disarm(disarmReason_t disarmReason)

        statsOnDisarm();
        logicConditionReset();
+        programmingPidReset();
        beeper(BEEPER_DISARMING);      // emit disarm tone
    }
 }
@ -480,6 +482,7 @@ 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();
+        programmingPidReset();
        headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);

        resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@ -148,8 +148,6 @@

 #include "telemetry/telemetry.h"

-#include "uav_interconnect/uav_interconnect.h"
-
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
 #endif
@ -218,7 +216,7 @@ void init(void)
 #endif

    initEEPROM();
-    //ensureEEPROMContainsValidData();
+    ensureEEPROMContainsValidData();
    readEEPROM();

    // Re-initialize system clock to their final values (if necessary)
@ -570,10 +568,6 @@ void init(void)
    }
 #endif

-#ifdef USE_UAV_INTERCONNECT
-    uavInterconnectBusInit();
-#endif
-
 #if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
    // Register the srxl Textgen telemetry sensor as a displayport device
    cmsDisplayPortRegister(displayPortSrxlInit());
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@ -36,6 +36,7 @@
 #include "common/time.h"
 #include "common/utils.h"
 #include "programming/global_variables.h"
+#include "programming/pid.h"

 #include "config/parameter_group_ids.h"

@ -553,6 +554,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
            sbufWriteU32(dst, gvGet(i));
        }
        break;
+    case MSP2_INAV_PROGRAMMING_PID:
+        for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+            sbufWriteU8(dst, programmingPids(i)->enabled);
+            sbufWriteU8(dst, programmingPids(i)->setpoint.type);
+            sbufWriteU32(dst, programmingPids(i)->setpoint.value);
+            sbufWriteU8(dst, programmingPids(i)->measurement.type);
+            sbufWriteU32(dst, programmingPids(i)->measurement.value);
+            sbufWriteU16(dst, programmingPids(i)->gains.P);
+            sbufWriteU16(dst, programmingPids(i)->gains.I);
+            sbufWriteU16(dst, programmingPids(i)->gains.D);
+            sbufWriteU16(dst, programmingPids(i)->gains.FF);
+        }
+        break;
 #endif
    case MSP2_COMMON_MOTOR_MIXER:
        for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@ -686,18 +700,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF

    case MSP_PID:
        for (int i = 0; i < PID_ITEM_COUNT; i++) {
-            sbufWriteU8(dst, pidBank()->pid[i].P);
-            sbufWriteU8(dst, pidBank()->pid[i].I);
-            sbufWriteU8(dst, pidBank()->pid[i].D);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
        }
        break;

    case MSP2_PID:
        for (int i = 0; i < PID_ITEM_COUNT; i++) {
-            sbufWriteU8(dst, pidBank()->pid[i].P);
-            sbufWriteU8(dst, pidBank()->pid[i].I);
-            sbufWriteU8(dst, pidBank()->pid[i].D);
-            sbufWriteU8(dst, pidBank()->pid[i].FF);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255));
        }
        break;

@ -810,6 +824,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
        sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
        break;

+    case MSP2_INAV_MISC2:
+        // Timers
+        sbufWriteU32(dst, micros() / 1000000); // On time (seconds)
+        sbufWriteU32(dst, getFlightTime()); // Flight time (seconds)
+
+        // Throttle
+        sbufWriteU8(dst, getThrottlePercent()); // Throttle Percent
+        sbufWriteU8(dst, navigationIsControllingThrottle() ? 1 : 0); // Auto Throttle Flag (0 or 1)
+
+        break;
+
    case MSP2_INAV_BATTERY_CONFIG:
        sbufWriteU16(dst, batteryMetersConfig()->voltage.scale);
        sbufWriteU8(dst, batteryMetersConfig()->voltageSource);
@ -1515,6 +1540,20 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
    return true;
 }

+static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
+{
+    const uint8_t safe_home_no = sbufReadU8(src);    // get the home number
+    if(safe_home_no < MAX_SAFE_HOMES) {
+        sbufWriteU8(dst, safe_home_no);
+        sbufWriteU8(dst, safeHomeConfig(safe_home_no)->enabled);
+        sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lat);
+        sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lon);
+        return MSP_RESULT_ACK;
+    } else {
+         return MSP_RESULT_ERROR;
+    }
+}
+
 #ifdef USE_NAV
 static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
 {
@ -1955,6 +1994,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
        } else
            return MSP_RESULT_ERROR;
        break;
+
+    case MSP2_INAV_SET_PROGRAMMING_PID:
+        sbufReadU8Safe(&tmp_u8, src);
+        if ((dataSize == 20) && (tmp_u8 < MAX_PROGRAMMING_PID_COUNT)) {
+            programmingPidsMutable(tmp_u8)->enabled = sbufReadU8(src);
+            programmingPidsMutable(tmp_u8)->setpoint.type = sbufReadU8(src);
+            programmingPidsMutable(tmp_u8)->setpoint.value = sbufReadU32(src);
+            programmingPidsMutable(tmp_u8)->measurement.type = sbufReadU8(src);
+            programmingPidsMutable(tmp_u8)->measurement.value = sbufReadU32(src);
+            programmingPidsMutable(tmp_u8)->gains.P = sbufReadU16(src);
+            programmingPidsMutable(tmp_u8)->gains.I = sbufReadU16(src);
+            programmingPidsMutable(tmp_u8)->gains.D = sbufReadU16(src);
+            programmingPidsMutable(tmp_u8)->gains.FF = sbufReadU16(src);
+        } else
+            return MSP_RESULT_ERROR;
+        break;
 #endif
    case MSP2_COMMON_SET_MOTOR_MIXER:
        sbufReadU8Safe(&tmp_u8, src);
@ -2892,6 +2947,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
        return MSP_RESULT_ERROR; // will only be reached if the rollback is not ready
        break;
 #endif
+    case MSP2_INAV_SET_SAFEHOME:
+        if (dataSize == 10) {
+             uint8_t i;
+             if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) {
+                 return MSP_RESULT_ERROR;
+             }
+             safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
+             safeHomeConfigMutable(i)->lat = sbufReadU32(src);
+             safeHomeConfigMutable(i)->lon = sbufReadU32(src);
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+        break;

    default:
        return MSP_RESULT_ERROR;
@ -3195,6 +3263,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
        break;
 #endif

+    case MSP2_INAV_SAFEHOME:
+         *ret = mspFcSafeHomeOutCommand(dst, src);
+         break;
+
    default:
        // Not handled
        return false;
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@ -49,6 +49,7 @@
 #include "flight/wind_estimator.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
+#include "flight/dynamic_lpf.h"

 #include "navigation/navigation.h"

@ -90,8 +91,6 @@

 #include "config/feature.h"

-#include "uav_interconnect/uav_interconnect.h"
-
 void taskHandleSerial(timeUs_t currentTimeUs)
 {
    UNUSED(currentTimeUs);
@ -294,6 +293,7 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 {
    UNUSED(currentTimeUs);
    updatePIDCoefficients();
+    dynamicLpfGyroTask();
 }

 void fcTasksInit(void)
@ -346,7 +346,7 @@ void fcTasksInit(void)
    setTaskEnabled(TASK_STACK_CHECK, true);
 #endif
 #if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS)
-    setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS));
+    setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM));
 #endif
 #ifdef USE_CMS
 #ifdef USE_MSP_DISPLAYPORT
@ -363,9 +363,6 @@ void fcTasksInit(void)
    setTaskEnabled(TASK_VTXCTRL, true);
 #endif
 #endif
-#ifdef USE_UAV_INTERCONNECT
-    setTaskEnabled(TASK_UAV_INTERCONNECT, uavInterconnectBusIsInitialized());
-#endif
 #ifdef USE_RCDEVICE
    setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
 #endif
@ -570,16 +567,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
    [TASK_OPFLOW] = {
        .taskName = "OPFLOW",
        .taskFunc = taskUpdateOpticalFlow,
-        .desiredPeriod = TASK_PERIOD_HZ(100),   // I2C/SPI sensor will work at higher rate and accumulate, UIB/UART sensor will work at lower rate w/o accumulation
-        .staticPriority = TASK_PRIORITY_MEDIUM,
-    },
-#endif
-
-#ifdef USE_UAV_INTERCONNECT
-    [TASK_UAV_INTERCONNECT] = {
-        .taskName = "UIB",
-        .taskFunc = uavInterconnectBusTask,
-        .desiredPeriod = 1000000 / 500,          // 500 Hz
+        .desiredPeriod = TASK_PERIOD_HZ(100),   // I2C/SPI sensor will work at higher rate and accumulate, UART sensor will work at lower rate w/o accumulation
        .staticPriority = TASK_PRIORITY_MEDIUM,
    },
 #endif
@ -620,7 +608,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
    [TASK_AUX] = {
        .taskName = "AUX",
        .taskFunc = taskUpdateAux,
-        .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 300Hz @3,33ms
+        .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 100Hz @10ms
        .staticPriority = TASK_PRIORITY_HIGH,
    },
 };
--- a/src/main/fc/rc_adjustments.c
+++ b/src/main/fc/rc_adjustments.c
@ -360,9 +360,9 @@ static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, u
    return applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_MANUAL_RATE_MIN, RC_ADJUSTMENT_MANUAL_RATE_MAX);
 }

-static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
+static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta)
 {
-    applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
+    applyAdjustmentU16(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
 }

 static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
--- a/src/main/fc/runtime_config.c
+++ b/src/main/fc/runtime_config.c
@ -144,6 +144,9 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
    if (FLIGHT_MODE(MANUAL_MODE))
        return FLM_MANUAL;

+    if (FLIGHT_MODE(NAV_LAUNCH_MODE))
+        return FLM_LAUNCH;
+
    if (FLIGHT_MODE(NAV_RTH_MODE))
        return FLM_RTH;

@ -165,8 +168,6 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
    if (FLIGHT_MODE(HORIZON_MODE))
        return FLM_HORIZON;

-    if (FLIGHT_MODE(NAV_LAUNCH_MODE))
-        return FLM_LAUNCH;

    return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
 }
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@ -7,7 +7,7 @@ tables:
    values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"]
    enum: accelerationSensor_e
  - name: rangefinder_hardware
-    values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"]
+    values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"]
    enum: rangefinderType_e
  - name: mag_hardware
    values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"]
@ -22,7 +22,7 @@ tables:
    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
    enum: pitotSensor_e
  - name: receiver_type
-    values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
+    values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UNUSED"]
    enum: rxReceiverType_e
  - name: serial_rx
    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST"]
@ -34,7 +34,7 @@ tables:
  - name: motor_pwm_protocol
    values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"]
  - name: servo_protocol
-    values: ["PWM", "SERVO_DRIVER", "SBUS"]
+    values: ["PWM", "SERVO_DRIVER", "SBUS", "SBUS_PWM"]
  - name: failsafe_procedure
    values: ["SET-THR", "DROP", "RTH", "NONE"]
  - name: current_sensor
@ -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"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF"]
  - name: async_mode
    values: ["NONE", "GYRO", "ALL"]
  - name: aux_operator
@ -155,6 +155,8 @@ tables:
  - name: nav_overrides_motor_stop
    enum: navOverridesMotorStop_e
    values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"]
+  - name: osd_plus_code_short
+    values: ["0", "2", "4", "6"]

 groups:
  - name: PG_GYRO_CONFIG
@ -214,6 +216,29 @@ groups:
        default_value: "`BIQUAD`"
        field: gyro_stage2_lowpass_type
        table: filter_type
+      - name: gyro_use_dyn_lpf
+        description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."
+        default_value: "OFF"
+        field: useDynamicLpf
+        type: bool
+      - name: gyro_dyn_lpf_min_hz
+        description: "Minimum frequency of the gyro Dynamic LPF"
+        default_value: "200"
+        field: gyroDynamicLpfMinHz
+        min: 40
+        max: 400
+      - name: gyro_dyn_lpf_max_hz
+        description: "Maximum frequency of the gyro Dynamic LPF"
+        default_value: "500"
+        field: gyroDynamicLpfMaxHz
+        min: 40
+        max: 1000
+      - name: gyro_dyn_lpf_curve_expo
+        description: "Expo value for the throttle-to-frequency mapping for Dynamic LPF"
+        default_value: "5"
+        field: gyroDynamicLpfCurveExpo
+        min: 1
+        max: 10
      - name: dynamic_gyro_notch_enabled
        description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
        default_value: "`OFF`"
@ -1317,13 +1342,13 @@ groups:
        field: mid_throttle_deadband
        min: 0
        max: 200
-      - name: mc_airmode_type
-        description: "Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode."
+      - name: airmode_type
+        description: "Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes."
        default_value: "STICK_CENTER"
        field: airmodeHandlingType
        table: airmodeHandlingType
-      - name: mc_airmode_threshold
-        description: "Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used"
+      - name: airmode_throttle_threshold
+        description: "Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used"
        default_value: "1300"
        field: airmodeThrottleThreshold
        min: 1000
@ -1849,6 +1874,12 @@ groups:
        condition: USE_GYRO_KALMAN
        min: 1
        max: 16000
+      - name: fw_level_pitch_trim
+        description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level"
+        default_value: "0"
+        field: fixedWingLevelTrim
+        min: -10
+        max: 10

  - name: PG_PID_AUTOTUNE_CONFIG
    type: pidAutotuneConfig_t
@ -2107,7 +2138,7 @@ groups:
        min: 0
        max: 5000
      - name: nav_overrides_motor_stop
-        description: "When set OFF_ALWAYS 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"
+        description: "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"
        default_value: "ALL_NAV"
        field: general.flags.nav_overrides_motor_stop
        table: nav_overrides_motor_stop
@ -2309,7 +2340,7 @@ groups:
        max: 900
      - name: nav_fw_loiter_radius
        description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]"
-        default_value: "5000"
+        default_value: "7500"
        field: fw.loiter_radius
        min: 0
        max: 10000
@ -2923,9 +2954,17 @@ groups:
        type: bool

      - name: osd_plus_code_digits
+        description: "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."
        field: plus_code_digits
+        default_value: 10
        min: 10
        max: 13
+      - name: osd_plus_code_short
+        description: "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."
+        field: plus_code_short
+        default_value: "0"
+        table: osd_plus_code_short
+        type: uint8_t

      - name: osd_ahi_style
        description: "Sets OSD Artificial Horizon style \"DEFAULT\" or \"LINE\" for the FrSky Graphical OSD."
@ -2982,6 +3021,10 @@ groups:
        default_value: 0
        description: Same as left_sidebar_scroll_step, but for the right sidebar

+      - name: osd_home_position_arm_screen
+        type: bool
+        default_value: "ON"
+        description: Should home position coordinates be displayed on the arming screen.

  - name: PG_SYSTEM_CONFIG
    type: systemConfig_t
@ -3080,6 +3123,11 @@ groups:
        default_value: "ON"
        field: halfDuplex
        type: bool
+      - name: vtx_smartaudio_early_akk_workaround
+        description: "Enable workaround for early AKK SAudio-enabled VTX bug."
+        default_value: "ON"
+        field: smartAudioEarlyAkkWorkaroundEnable
+        type: bool

  - name: PG_VTX_SETTINGS_CONFIG
    type: vtxSettingsConfig_t
--- a/src/main/flight/dynamic_lpf.c
+++ b/src/main/flight/dynamic_lpf.c
@ -0,0 +1,50 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "flight/dynamic_lpf.h"
+#include "sensors/gyro.h"
+#include "flight/mixer.h"
+#include "fc/rc_controls.h"
+#include "build/debug.h"
+
+static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
+    const float expof = expo / 10.0f;
+    static float curve;
+    curve = throttle * (1 - throttle) * expof + throttle;
+    return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
+}
+
+void dynamicLpfGyroTask(void) {
+
+    if (!gyroConfig()->useDynamicLpf) {
+        return;
+    }
+
+    const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
+    const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
+
+    DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq);
+
+    gyroUpdateDynamicLpf(cutoffFreq);
+}
--- a/src/main/flight/dynamic_lpf.h
+++ b/src/main/flight/dynamic_lpf.h
@ -1,5 +1,5 @@
 /*
- * This file is part of INAV.
+ * This file is part of INAV Project.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this file,
@ -20,12 +20,10 @@
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see http://www.gnu.org/licenses/.
- *
- * @author Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
 */

 #pragma once

-#include "rx/rx.h"
+#include <stdint.h>

-void rxUIBInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+void dynamicLpfGyroTask(void);
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@ -572,6 +572,12 @@ void FAST_CODE mixTable(const float dT)
    motorRateLimitingApplyFn(dT);
 }

+int16_t getThrottlePercent(void)
+{
+    int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - getThrottleIdleValue()) * 100 / (motorConfig()->maxthrottle - getThrottleIdleValue());
+    return thr;
+}
+
 motorStatus_e getMotorStatus(void)
 {
    if (failsafeRequiresMotorStop()) {
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@ -113,6 +113,7 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 extern int mixerThrottleCommand;

 int getThrottleIdleValue(void);
+int16_t getThrottlePercent(void);
 uint8_t getMotorCount(void);
 float getMotorMixRange(void);
 bool mixerIsOutputSaturated(void);
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@ -30,6 +30,7 @@ FILE_COMPILE_FOR_SPEED
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
+#include "common/fp_pid.h"

 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@ -153,8 +154,9 @@ static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
+static EXTENDED_FASTRAM float fixedWingLevelTrim;

-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1);

 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
        .bank_mc = {
@ -279,6 +281,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
        .kalman_w = 4,
        .kalman_sharpness = 100,
        .kalmanEnabled = 0,
+        .fixedWingLevelTrim = 0,
 );

 bool pidInitFilters(void)
@ -533,6 +536,21 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
    if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
        angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);

+
+    //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
+    if (axis == FD_PITCH && STATE(AIRPLANE)) {
+        /* 
+         * fixedWingLevelTrim has opposite sign to rcCommand.
+         * Positive rcCommand means nose should point downwards
+         * Negative rcCommand mean nose should point upwards
+         * This is counter intuitive and a natural way suggests that + should mean UP
+         * This is why fixedWingLevelTrim has opposite sign to rcCommand
+         * Positive fixedWingLevelTrim means nose should point upwards
+         * Negative fixedWingLevelTrim means nose should point downwards
+         */
+        angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);   
+    }
+
    const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);

    float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
@ -1107,6 +1125,8 @@ void pidInit(void)
        gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness);
    }
 #endif
+
+    fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim;
 }

 const pidBank_t * pidBank(void) { 
@ -1116,7 +1136,7 @@ pidBank_t * pidBankMutable(void) {
    return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }

-uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
+uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
 {
    if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) {
       return &pidBank->pid[pidIndex].FF;
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@ -81,10 +81,10 @@ typedef enum {
 } pidType_e;

 typedef struct pid8_s {
-    uint8_t P;
-    uint8_t I;
-    uint8_t D;
-    uint8_t FF;
+    uint16_t P;
+    uint16_t I;
+    uint16_t D;
+    uint16_t FF;
 } pid8_t;

 typedef struct pidBank_s {
@ -150,6 +150,8 @@ typedef struct pidProfile_s {
    uint16_t kalman_w;
    uint16_t kalman_sharpness;
    uint8_t kalmanEnabled;
+
+    float fixedWingLevelTrim;
 } pidProfile_t;

 typedef struct pidAutotuneConfig_s {
@ -199,4 +201,4 @@ void autotuneUpdateState(void);
 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);

 pidType_e pidIndexGetType(pidIndex_e pidIndex);
-uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);
+uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@ -133,7 +133,7 @@ void servosInit(void)
 {
    // give all servos a default command
    for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
-        servo[i] = DEFAULT_SERVO_MIDDLE;
+        servo[i] = servoParams(i)->middle;
    }

    /*
--- a/src/main/io/asyncfatfs/asyncfatfs.c
+++ b/src/main/io/asyncfatfs/asyncfatfs.c
@ -3252,6 +3252,7 @@ uint32_t afatfs_freadSync(afatfsFilePtr_t file, uint8_t *buffer, uint32_t length
        uint32_t leftToRead = length - bytesRead;
        uint32_t readNow = afatfs_fread(file, buffer, leftToRead);
        bytesRead += readNow;
+        buffer += readNow;
        if (bytesRead < length) {

            if (afatfs_feof(file)) {
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@ -21,6 +21,7 @@
 #include <ctype.h>
 #include <string.h>
 #include <math.h>
+#include <stdarg.h>

 #include "platform.h"
 #include "build/build_config.h"
@ -67,8 +68,18 @@
 #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))

 // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
+// note PRNs last upadted 2020-12-18
+
+#define SBASMASK1_BASE 120
+#define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE))
+
 static const uint32_t ubloxScanMode1[] = {
-    0x00000000, 0x00000851, 0x0004E004, 0x00020200, 0x00000180, 0x00000000,
+    0x00000000, // AUTO
+    (SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136)), // SBAS
+    (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(138)), // WAAS
+    (SBASMASK1_BITS(129) | SBASMASK1_BITS(137)), // MAAS
+    (SBASMASK1_BITS(127) | SBASMASK1_BITS(128)), // GAGAN
+    0x00000000, // NONE
 };

 static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
@ -101,11 +112,35 @@ typedef struct {
    uint16_t time;
 } ubx_rate;

+typedef struct {
+     uint8_t gnssId;
+     uint8_t resTrkCh;
+     uint8_t maxTrkCh;
+     uint8_t reserved1;
+// flags
+     uint8_t enabled;
+     uint8_t undefined0;
+     uint8_t sigCfgMask;
+     uint8_t undefined1;
+} ubx_gnss_element_t;
+
+typedef struct {
+     uint8_t msgVer;
+     uint8_t numTrkChHw;
+     uint8_t numTrkChUse;
+     uint8_t numConfigBlocks;
+     ubx_gnss_element_t config[0];
+} ubx_gnss_msg_t;
+
+#define MAX_GNSS 7
+#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
+
 typedef union {
-    uint8_t bytes[60]; // sizeof Galileo config
+    uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder
    ubx_sbas sbas;
    ubx_msg msg;
    ubx_rate rate;
+    ubx_gnss_msg_t gnss;
 } ubx_payload;

 // UBX support
@ -409,25 +444,63 @@ static const uint8_t default_payload[] = {
    0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
 };

-// Note the organisation of the bytes reflects the structure of the payload
-// 4 bytes then 8*number of elements (7)
-static const uint8_t galileo_payload[] =  {
-    0x00, 0x00, 0x20, 0x07,			    // GNSS    / min / max / enable
-    0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS     / 8 / 16 / Y
-    0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS    / 1 /  3 / Y
-    0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo / 4 /  8 / Y
-    0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou  / 8 / 16 / N
-    0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01, // IMES    / 0 /  8 / N
-    0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01, // QZSS    / 0 /  3 / N
-    0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01  // GLONASS / 8 / 14 / Y
-};
+#define GNSSID_SBAS 1
+#define GNSSID_GALILEO 2

-static void configureGalileo(void)
+static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
 {
+    gnss_block->gnssId = GNSSID_SBAS;
+    gnss_block->maxTrkCh = 3;
+    gnss_block->sigCfgMask = 1;
+    if (gpsState.gpsConfig->sbasMode == SBAS_NONE) {
+         gnss_block->enabled = 0;
+         gnss_block->resTrkCh = 0;
+    } else {
+         gnss_block->enabled = 1;
+         gnss_block->resTrkCh = 1;
+    }
+
+    return 1;
+}
+
+static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
+{
+    if (!capGalileo) {
+        return 0;
+    }
+
+    gnss_block->gnssId = GNSSID_GALILEO;
+    gnss_block->maxTrkCh = 8;
+    gnss_block->sigCfgMask = 1;
+    if (gpsState.gpsConfig->ubloxUseGalileo) {
+        gnss_block->enabled = 1;
+        gnss_block->resTrkCh = 4;
+    } else {
+        gnss_block->enabled = 0;
+        gnss_block->resTrkCh = 0;
+    }
+
+    return 1;
+}
+
+static void configureGNSS(void)
+{
+    int blocksUsed = 0;
+
    send_buffer.message.header.msg_class = CLASS_CFG;
    send_buffer.message.header.msg_id = MSG_CFG_GNSS;
-    send_buffer.message.header.length = sizeof(galileo_payload);
-    memcpy(send_buffer.message.payload.bytes, galileo_payload, sizeof(galileo_payload));
+    send_buffer.message.payload.gnss.msgVer = 0;
+    send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
+    send_buffer.message.payload.gnss.numTrkChUse = 32;
+
+    /* SBAS, always generated */
+    blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
+
+    /* Galileo */
+    blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
+
+    send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
+    send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed);
    sendConfigMessageUBLOX();
 }

@ -578,7 +651,16 @@ static bool gpsParceFrameUBLOX(void)
            // EXT CORE 3.01 (107900)
            // 01234567890123456789012
            gpsState.hwVersion = fastA2I(_buffer.ver.hwVersion);
-            capGalileo = ((gpsState.hwVersion >= 80000) && (_buffer.ver.swVersion[9] > '2')); // M8N and SW major 3 or later
+            if  ((gpsState.hwVersion >= 80000) && (_buffer.ver.swVersion[9] > '2')) {
+                // check extensions;
+                // after hw + sw vers; each is 30 bytes
+                for(int j = 40; j < _payload_length; j += 30) {
+                    if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) {
+                        capGalileo = true;
+                        break;
+                    }
+                }
+            }
        }
        break;
    case MSG_ACK_ACK:
@ -840,13 +922,11 @@ STATIC_PROTOTHREAD(gpsConfigure)
    configureSBAS();
    ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);

-    // Enable GALILEO
-    if (gpsState.gpsConfig->ubloxUseGalileo && capGalileo) {
-        // If GALILEO is not supported by the hardware we'll get a NAK,
-        // however GPS would otherwise be perfectly initialized, so we are just waiting for any response
-        gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
-        configureGalileo();
-        ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
+    // Configure GNSS for M8N and later
+    if (gpsState.hwVersion >= 80000) {
+         gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
+         configureGNSS();
+         ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
    }

    ptEnd(0);
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@ -185,7 +185,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, 13);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);

 static int digitCount(int32_t value)
@ -869,18 +869,15 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y)
 **/
 static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr)
 {
-    const int minThrottle = getThrottleIdleValue();
    buff[0] = SYM_BLANK;
    buff[1] = SYM_THR;
-    int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle);
    if (autoThr && navigationIsControllingThrottle()) {
        buff[0] = SYM_AUTO_THR0;
        buff[1] = SYM_AUTO_THR1;
-        thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
        if (isFixedWingAutoThrottleManuallyIncreased())
            TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
    }
-    tfp_sprintf(buff + 2, "%3d", thr);
+    tfp_sprintf(buff + 2, "%3d", getThrottlePercent());
 }

 /**
@ -2037,6 +2034,11 @@ static bool osdDrawSingleElement(uint8_t item)
            osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3);
            buff[3] = SYM_WATT;
            buff[4] = '\0';
+
+            uint8_t current_alarm = osdConfig()->current_alarm;
+            if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) {
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            }
            break;
        }

@ -2067,6 +2069,13 @@ static bool osdDrawSingleElement(uint8_t item)
            break;
        }

+    case OSD_VERSION:
+        {
+            tfp_sprintf(buff, "INAV %s", FC_VERSION_STRING);
+            displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
+            break;
+        }
+
    case OSD_MAIN_BATT_CELL_VOLTAGE:
        {
            osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), 3, 2);
@ -2101,8 +2110,7 @@ static bool osdDrawSingleElement(uint8_t item)
    case OSD_EFFICIENCY_MAH_PER_KM:
        {
            // amperage is in centi amps, speed is in cms/s. We want
-            // mah/km. Values over 999 are considered useless and
-            // displayed as "---""
+            // mah/km. Only show when ground speed > 1m/s.
            static pt1Filter_t eFilterState;
            static timeUs_t efficiencyUpdated = 0;
            int32_t value = 0;
@ -2118,7 +2126,7 @@ static bool osdDrawSingleElement(uint8_t item)
                    value = eFilterState.state;
                }
            }
-            if (value > 0 && value <= 999) {
+            if (value > 0 && gpsSol.groundSpeed > 100) {
                tfp_sprintf(buff, "%3d", (int)value);
            } else {
                buff[0] = buff[1] = buff[2] = '-';
@ -2132,8 +2140,7 @@ static bool osdDrawSingleElement(uint8_t item)
    case OSD_EFFICIENCY_WH_PER_KM:
        {
            // amperage is in centi amps, speed is in cms/s. We want
-            // mWh/km. Values over 999Wh/km are considered useless and
-            // displayed as "---""
+            // mWh/km. Only show when ground speed > 1m/s.
            static pt1Filter_t eFilterState;
            static timeUs_t efficiencyUpdated = 0;
            int32_t value = 0;
@ -2149,7 +2156,7 @@ static bool osdDrawSingleElement(uint8_t item)
                    value = eFilterState.state;
                }
            }
-            if (value > 0 && value <= 999999) {
+            if (value > 0 && gpsSol.groundSpeed > 100) {
                osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3);
            } else {
                buff[0] = buff[1] = buff[2] = '-';
@ -2275,6 +2282,7 @@ static bool osdDrawSingleElement(uint8_t item)
        {
            STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier);
            int digits = osdConfig()->plus_code_digits;
+            int digitsRemoved = osdConfig()->plus_code_short * 2;
            if (STATE(GPS_FIX)) {
                olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff));
            } else {
@ -2284,6 +2292,9 @@ static bool osdDrawSingleElement(uint8_t item)
                buff[8] = '+';
                buff[digits + 1] = '\0';
            }
+            // Optionally trim digits from the left
+            memmove(buff, buff+digitsRemoved, strlen(buff) + digitsRemoved);
+            buff[digits + 1 - digitsRemoved] = '\0';
            break;
        }

@ -2419,21 +2430,21 @@ static bool osdDrawSingleElement(uint8_t item)
            char buff[4];
            textAttributes_t attr;

-            displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA BP");
-
+            displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA");
            attr = TEXT_ATTRIBUTES_NONE;
-            tfp_sprintf(buff, "TPA  %3d", currentControlRateProfile->throttle.dynPID);
+            tfp_sprintf(buff, "%3d", currentControlRateProfile->throttle.dynPID);
            if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) {
                TEXT_ATTRIBUTES_ADD_BLINK(attr);
            }
-            displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, attr);
+            displayWriteWithAttr(osdDisplayPort, elemPosX + 5, elemPosY, buff, attr);

+            displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP");
            attr = TEXT_ATTRIBUTES_NONE;
-            tfp_sprintf(buff, "BP  %4d", currentControlRateProfile->throttle.pa_breakpoint);
+            tfp_sprintf(buff, "%4d", currentControlRateProfile->throttle.pa_breakpoint);
            if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) {
                TEXT_ATTRIBUTES_ADD_BLINK(attr);
            }
-            displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY + 1, buff, attr);
+            displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY + 1, buff, attr);

            return true;
        }
@ -2581,6 +2592,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
    .left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE,
    .right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE,
    .sidebar_scroll_arrows = 0,
+    .osd_home_position_arm_screen = true,

    .units = OSD_UNIT_METRIC,
    .main_voltage_decimals = 1,
@ -2591,6 +2603,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
    .osd_failsafe_switch_layout = false,

    .plus_code_digits = 11,
+    .plus_code_short = 0,

    .ahi_width = OSD_AHI_WIDTH * OSD_CHAR_WIDTH,
    .ahi_height = OSD_AHI_HEIGHT * OSD_CHAR_HEIGHT,
@ -2930,7 +2943,7 @@ static void osdShowStats(void)
    if (osdDisplayIsPAL())
        displayWrite(osdDisplayPort, statNameX, top++, "  --- STATS ---");

-    if (STATE(GPS_FIX)) {
+    if (feature(FEATURE_GPS)) {
        displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED        :");
        osdFormatVelocityStr(buff, stats.max_speed, true);
        osdLeftAlignString(buff);
@ -2982,7 +2995,7 @@ static void osdShowStats(void)
        displayWrite(osdDisplayPort, statValuesX, top++, buff);

        int32_t totalDistance = getTotalTravelDistance();
-        if (totalDistance > 0) {
+        if (feature(FEATURE_GPS)) {
            displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY   :");
            if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH)
                tfp_sprintf(buff, "%d%c%c", (int)(getMAhDrawn() * 100000 / totalDistance),
@ -2993,6 +3006,19 @@ static void osdShowStats(void)
                buff[4] = SYM_WH_KM_1;
                buff[5] = '\0';
            }
+            // If traveled distance is less than 100 meters efficiency numbers are useless and unreliable so display --- instead
+            if (totalDistance < 10000) {
+                buff[0] = buff[1] = buff[2] = '-';
+                if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH){
+                    buff[3] = SYM_MAH_KM_0;
+                    buff[4] = SYM_MAH_KM_1;
+                    buff[5] = '\0';
+                } else {
+                    buff[3] = SYM_WH_KM_0;
+                    buff[4] = SYM_WH_KM_1;
+                    buff[5] = '\0';
+                }
+            }
            displayWrite(osdDisplayPort, statValuesX, top++, buff);
        }
    }
@ -3030,10 +3056,11 @@ static void osdShowArmed(void)
    dateTime_t dt;
    char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)];
    char craftNameBuf[MAX_NAME_LENGTH];
+    char versionBuf[30];
    char *date;
    char *time;
-    // We need 10 visible rows
-    uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 10 - 1);
+    // We need 12 visible rows
+    uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 12 - 1);

    displayClearScreen(osdDisplayPort);
    displayWrite(osdDisplayPort, 12, y, "ARMED");
@ -3048,13 +3075,15 @@ static void osdShowArmed(void)
 #if defined(USE_GPS)
    if (feature(FEATURE_GPS)) {
        if (STATE(GPS_FIX_HOME)) {
-            osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
-            displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
-            osdFormatCoordinate(buf, SYM_LON, GPS_home.lon);
-            displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf);
-            int digits = osdConfig()->plus_code_digits;
-            olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
-            displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf);
+            if (osdConfig()->osd_home_position_arm_screen){
+                osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
+                displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
+                osdFormatCoordinate(buf, SYM_LON, GPS_home.lon);
+                displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf);
+                int digits = osdConfig()->plus_code_digits;
+                olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
+                displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf);
+            }
            y += 4;
 #if defined (USE_SAFE_HOME)
            if (isSafeHomeInUse()) {
@ -3080,7 +3109,11 @@ static void osdShowArmed(void)

        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date);
        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time);
+        y += 3;
    }
+
+    tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING);
+    displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, y, versionBuf);
 }

 static void osdFilterData(timeUs_t currentTimeUs) {
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@ -218,6 +218,7 @@ typedef enum {
    OSD_GVAR_3,
    OSD_TPA,
    OSD_NAV_FW_CONTROL_SMOOTHNESS,
+    OSD_VERSION,
    OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;

@ -336,6 +337,7 @@ typedef struct osdConfig_s {

    bool osd_failsafe_switch_layout;
    uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE
+    uint8_t plus_code_short; 
    uint8_t osd_ahi_style;
    uint8_t force_grid;                 // Force a pixel based OSD to use grid mode.
    uint8_t ahi_bordered;               // Only used by the AHI widget
@ -345,6 +347,7 @@ typedef struct osdConfig_s {
    int8_t sidebar_horizontal_offset;   // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges.
    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 crsf_lq_format;

--- a/src/main/io/osd_canvas.c
+++ b/src/main/io/osd_canvas.c
@ -647,7 +647,7 @@ static bool osdCanvasDrawSidebar(uint32_t *configured, displayWidgets_t *widgets
            .options = options,
            .divisions = OSD_AH_SIDEBAR_HEIGHT_POS * 2,
        };
-        uint16_t countsPerStep;
+        uint16_t countsPerStep = 0;
        osdCanvasSidebarGetUnit(&config.unit, &countsPerStep, scroll);

        int center = ex * OSD_CHAR_WIDTH;
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@ -44,7 +44,7 @@ typedef enum {
    FUNCTION_RCDEVICE                   = (1 << 10), // 1024
    FUNCTION_VTX_SMARTAUDIO             = (1 << 11), // 2048
    FUNCTION_VTX_TRAMP                  = (1 << 12), // 4096
-    FUNCTION_UAV_INTERCONNECT           = (1 << 13), // 8192
+    FUNCTION_UNUSED_1                   = (1 << 13), // 8192: former UAV_INTERCONNECT
    FUNCTION_OPTICAL_FLOW               = (1 << 14), // 16384
    FUNCTION_LOG                        = (1 << 15), // 32768
    FUNCTION_RANGEFINDER                = (1 << 16), // 65536
--- a/src/main/io/vtx_control.c
+++ b/src/main/io/vtx_control.c
@ -39,10 +39,11 @@

 #if defined(USE_VTX_CONTROL)

-PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 3);

 PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
      .halfDuplex = true,
+      .smartAudioEarlyAkkWorkaroundEnable = true,
 );

 static uint8_t locked = 0;
--- a/src/main/io/vtx_control.h
+++ b/src/main/io/vtx_control.h
@ -31,6 +31,7 @@ typedef struct vtxChannelActivationCondition_s {
 typedef struct vtxConfig_s {
    vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT];
    uint8_t halfDuplex;
+    uint8_t smartAudioEarlyAkkWorkaroundEnable;
 } vtxConfig_t;

 PG_DECLARE(vtxConfig_t, vtxConfig);
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@ -506,7 +506,8 @@ static void saSendFrame(uint8_t *buf, int len)
    // XXX: Workaround for early AKK SAudio-enabled VTX bug,
    // shouldn't cause any problems with VTX with properly
    // implemented SAudio.
-    serialWrite(smartAudioSerialPort, 0x00);
+	//Update: causes problem with new AKK AIO camera connected to SoftUART
+    if (vtxConfig()->smartAudioEarlyAkkWorkaroundEnable) serialWrite(smartAudioSerialPort, 0x00);

    sa_lastTransmissionMs = millis();
    saStat.pktsent++;
--- a/src/main/msc/emfat_file.c
+++ b/src/main/msc/emfat_file.c
@ -411,7 +411,7 @@ static int emfat_find_log(emfat_entry_t *entry, int maxCount, int flashfsUsedSpa
                    char *last;
                    char* tok = strtok_r((char *)buffer, "-T:.", &last);
                    int index=0;
-                    int year=0,month,day,hour,min,sec;
+                    int year=0,month=0,day=0,hour=0,min=0,sec=0;
                    while (tok != NULL) {
                        switch(index) {
                            case 0:
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@ -61,6 +61,8 @@
 #define MSP2_INAV_SET_GLOBAL_FUNCTIONS          0x2025
 #define MSP2_INAV_LOGIC_CONDITIONS_STATUS       0x2026
 #define MSP2_INAV_GVAR_STATUS                   0x2027
+#define MSP2_INAV_PROGRAMMING_PID               0x2028
+#define MSP2_INAV_SET_PROGRAMMING_PID           0x2029

 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031
@ -72,3 +74,9 @@
 #define MSP2_INAV_FWUPDT_EXEC                   0x2035
 #define MSP2_INAV_FWUPDT_ROLLBACK_PREPARE       0x2036
 #define MSP2_INAV_FWUPDT_ROLLBACK_EXEC          0x2037
+
+#define MSP2_INAV_SAFEHOME                      0x2038
+#define MSP2_INAV_SET_SAFEHOME                  0x2039
+
+#define MSP2_INAV_MISC2                         0x203A
+
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@ -158,9 +158,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
        .max_throttle = 1700,
        .min_throttle = 1200,
        .pitch_to_throttle = 10,                // pwm units per degree of pitch (10pwm units ~ 1% throttle)
-        .pitch_to_throttle_smooth = 0,
-        .pitch_to_throttle_thresh = 0,
-        .loiter_radius = 5000,                  // 50m
+        .pitch_to_throttle_smooth = 6,
+        .pitch_to_throttle_thresh = 50,
+        .loiter_radius = 7500,                  // 75m

        //Fixed wing landing
        .land_dive_angle = 2,                   // 2 degrees dive by default
@ -1920,128 +1920,6 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
    return &posControl.rthState.homeTmpWaypoint;
 }

-/*-----------------------------------------------------------
- * Float point PID-controller implementation
- *-----------------------------------------------------------*/
-// Implementation of PID with back-calculation I-term anti-windup
-// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
-// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
-float navPidApply3(
-    pidController_t *pid,
-    const float setpoint,
-    const float measurement,
-    const float dt,
-    const float outMin,
-    const float outMax,
-    const pidControllerFlags_e pidFlags,
-    const float gainScaler,
-    const float dTermScaler
-) {
-    float newProportional, newDerivative, newFeedForward;
-    float error = setpoint - measurement;
-
-    /* P-term */
-    newProportional = error * pid->param.kP * gainScaler;
-
-    /* D-term */
-    if (pid->reset) {
-        pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement;
-        pid->reset = false;
-    }
-
-    if (pidFlags & PID_DTERM_FROM_ERROR) {
-        /* Error-tracking D-term */
-        newDerivative = (error - pid->last_input) / dt;
-        pid->last_input = error;
-    } else {
-        /* Measurement tracking D-term */
-        newDerivative = -(measurement - pid->last_input) / dt;
-        pid->last_input = measurement;
-    }
-
-    if (pid->dTermLpfHz > 0) {
-        newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
-    } else {
-        newDerivative = pid->param.kD * newDerivative;
-    }
-
-    newDerivative = newDerivative * gainScaler * dTermScaler;
-
-    if (pidFlags & PID_ZERO_INTEGRATOR) {
-        pid->integrator = 0.0f;
-    }
-
-    /*
-     * Compute FeedForward parameter
-     */
-    newFeedForward = setpoint * pid->param.kFF * gainScaler;
-
-    /* Pre-calculate output and limit it if actuator is saturating */
-    const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
-    const float outValConstrained = constrainf(outVal, outMin, outMax);
-
-    pid->proportional = newProportional;
-    pid->integral = pid->integrator;
-    pid->derivative = newDerivative;
-    pid->feedForward = newFeedForward;
-    pid->output_constrained = outValConstrained;
-
-    /* Update I-term */
-    if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
-        const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
-
-        if (pidFlags & PID_SHRINK_INTEGRATOR) {
-            // Only allow integrator to shrink
-            if (fabsf(newIntegrator) < fabsf(pid->integrator)) {
-                pid->integrator = newIntegrator;
-            }
-        }
-        else {
-            pid->integrator = newIntegrator;
-        }
-    }
-
-    return outValConstrained;
-}
-
-float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
-{
-    return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f);
-}
-
-void navPidReset(pidController_t *pid)
-{
-    pid->reset = true;
-    pid->proportional = 0.0f;
-    pid->integral = 0.0f;
-    pid->derivative = 0.0f;
-    pid->integrator = 0.0f;
-    pid->last_input = 0.0f;
-    pid->feedForward = 0.0f;
-    pt1FilterReset(&pid->dterm_filter_state, 0.0f);
-    pid->output_constrained = 0.0f;
-}
-
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz)
-{
-    pid->param.kP = _kP;
-    pid->param.kI = _kI;
-    pid->param.kD = _kD;
-    pid->param.kFF = _kFF;
-
-    if (_kI > 1e-6f && _kP > 1e-6f) {
-        float Ti = _kP / _kI;
-        float Td = _kD / _kP;
-        pid->param.kT = 2.0f / (Ti + Td);
-    }
-    else {
-        pid->param.kI = 0.0;
-        pid->param.kT = 0.0;
-    }
-    pid->dTermLpfHz = _dTermLpfHz;
-    navPidReset(pid);
-}
-
 /*-----------------------------------------------------------
 * Detects if thrust vector is facing downwards
 *-----------------------------------------------------------*/
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@ -23,6 +23,7 @@
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/vector.h"
+#include "common/fp_pid.h"

 #include "config/feature.h"

@ -325,33 +326,6 @@ typedef struct navDestinationPath_s {
    int32_t bearing; // deg * 100
 } navDestinationPath_t;

-typedef struct {
-    float kP;
-    float kI;
-    float kD;
-    float kT;   // Tracking gain (anti-windup)
-    float kFF;  // FeedForward Component
-} pidControllerParam_t;
-
-typedef struct {
-    float kP;
-} pControllerParam_t;
-
-typedef struct {
-    bool reset;
-    pidControllerParam_t param;
-    pt1Filter_t dterm_filter_state;     // last derivative for low-pass filter
-    float dTermLpfHz;                   // dTerm low pass filter cutoff frequency
-    float integrator;                   // integrator value
-    float last_input;                   // last input for derivative
-
-    float integral;                     // used integral value in output
-    float proportional;                 // used proportional value in output
-    float derivative;                   // used derivative value in output
-    float feedForward;                  // used FeedForward value in output
-    float output_constrained;           // controller output constrained
-} pidController_t;
-
 typedef struct navigationPIDControllers_s {
    /* Multicopter PIDs */
    pidController_t   pos[XYZ_AXIS_COUNT];
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@ -25,7 +25,9 @@
 #include "common/maths.h"
 #include "common/filter.h"
 #include "common/time.h"
+#include "common/vector.h"
 #include "fc/runtime_config.h"
+#include "navigation/navigation.h"

 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
 #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4       // low-pass filter on throttle output
@ -89,12 +91,6 @@ typedef struct navigationFlags_s {
    bool forcedRTHActivated;
 } navigationFlags_t;

-typedef enum {
-    PID_DTERM_FROM_ERROR            = 1 << 0,
-    PID_ZERO_INTEGRATOR             = 1 << 1,
-    PID_SHRINK_INTEGRATOR           = 1 << 2,
-} pidControllerFlags_e;
-
 typedef struct {
    fpVector3_t pos;
    fpVector3_t vel;
@ -389,21 +385,6 @@ extern multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 /* Internally used functions */
 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);

-float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
-float navPidApply3(
-    pidController_t *pid,
-    const float setpoint,
-    const float measurement,
-    const float dt,
-    const float outMin,
-    const float outMax,
-    const pidControllerFlags_e pidFlags,
-    const float gainScaler,
-    const float dTermScaler
-);
-void navPidReset(pidController_t *pid);
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);
-
 bool isThrustFacingDownwards(void);
 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);
 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos);
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@ -30,6 +30,7 @@

 #include "programming/logic_condition.h"
 #include "programming/global_variables.h"
+#include "programming/pid.h"
 #include "common/utils.h"
 #include "rx/rx.h"
 #include "common/maths.h"
@ -54,6 +55,7 @@ PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicCon

 EXTENDED_FASTRAM uint64_t logicConditionsGlobalFlags;
 EXTENDED_FASTRAM int logicConditionValuesByType[LOGIC_CONDITION_LAST];
+EXTENDED_FASTRAM rcChannelOverride_t rcChannelOverrides[MAX_SUPPORTED_RC_CHANNEL_COUNT];

 void pgResetFn_logicConditions(logicCondition_t *instance)
 {
@ -307,6 +309,14 @@ static int logicConditionCompute(
            return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB);
        break;

+        case LOGIC_CONDITION_RC_CHANNEL_OVERRIDE:
+            temporaryValue = constrain(operandA - 1, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1);
+            rcChannelOverrides[temporaryValue].active = true;
+            rcChannelOverrides[temporaryValue].value = constrain(operandB, PWM_RANGE_MIN, PWM_RANGE_MAX);
+            LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL);
+            return true;
+        break;
+
        default:
            return false;
            break; 
@ -595,6 +605,12 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
            }
            break;

+        case LOGIC_CONDITION_OPERAND_TYPE_PID:
+            if (operand >= 0 && operand < MAX_PROGRAMMING_PID_COUNT) {
+                retVal = programmingPidGetOutput(operand);
+            }
+            break;
+
        default:
            break;
    }
@ -619,9 +635,14 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) {
    //Disable all flags
    logicConditionsGlobalFlags = 0;

+    for (uint8_t i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
+        rcChannelOverrides[i].active = false;
+    }
+
    for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
        logicConditionProcess(i);
    }
+
 #ifdef USE_I2C_IO_EXPANDER
    ioPortExpanderSync();
 #endif
@ -663,3 +684,11 @@ int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {

    return outputValue;
 }
+
+int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
+    if (rcChannelOverrides[channel].active) {
+        return rcChannelOverrides[channel].value;
+    } else {
+        return originalValue;
+    }
+}
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@ -67,7 +67,8 @@ typedef enum {
    LOGIC_CONDITION_TAN                         = 35,
    LOGIC_CONDITION_MAP_INPUT                   = 36,
    LOGIC_CONDITION_MAP_OUTPUT                  = 37,
-    LOGIC_CONDITION_LAST                        = 38,
+    LOGIC_CONDITION_RC_CHANNEL_OVERRIDE         = 38,
+    LOGIC_CONDITION_LAST                        = 39,
 } logicOperation_e;

 typedef enum logicOperandType_s {
@ -77,6 +78,7 @@ typedef enum logicOperandType_s {
    LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE,
    LOGIC_CONDITION_OPERAND_TYPE_LC,    // Result of different LC and LC operand
    LOGIC_CONDITION_OPERAND_TYPE_GVAR,  // Value from a global variable
+    LOGIC_CONDITION_OPERAND_TYPE_PID,  // Value from a Programming PID
    LOGIC_CONDITION_OPERAND_TYPE_LAST
 } logicOperandType_e;

@ -141,6 +143,7 @@ typedef enum {
    LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW = (1 << 5),
    LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE = (1 << 6),
    LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7),
+    LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
 } logicConditionsGlobalFlags_t;

 typedef enum {
@ -168,6 +171,11 @@ typedef struct logicConditionState_s {
    uint8_t flags;
 } logicConditionState_t;

+typedef struct rcChannelOverride_s {
+    uint8_t active;
+    int value;
+} rcChannelOverride_t;
+
 extern int logicConditionValuesByType[LOGIC_CONDITION_LAST];
 extern uint64_t logicConditionsGlobalFlags;

@ -185,3 +193,4 @@ void logicConditionReset(void);

 float getThrottleScale(float globalThrottleScale);
 int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
+int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue);
--- a/src/main/programming/pid.c
+++ b/src/main/programming/pid.c
@ -0,0 +1,124 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "platform.h"
+
+FILE_COMPILE_FOR_SIZE
+
+#ifdef USE_PROGRAMMING_FRAMEWORK
+
+#include "common/utils.h"
+#include "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+#include "navigation/navigation_private.h"
+
+#include "programming/pid.h"
+#include "programming/logic_condition.h"
+
+EXTENDED_FASTRAM programmingPidState_t programmingPidState[MAX_PROGRAMMING_PID_COUNT];
+static bool pidsInitiated = false;
+
+PG_REGISTER_ARRAY_WITH_RESET_FN(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids, PG_PROGRAMMING_PID, 1);
+
+void pgResetFn_programmingPids(programmingPid_t *instance)
+{
+    for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        RESET_CONFIG(programmingPid_t, &instance[i],
+            .enabled = 0,
+            .setpoint = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .measurement = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .gains = {
+                .P = 0,
+                .I = 0,
+                .D = 0,
+                .FF = 0,
+            }
+        );
+    }
+}
+
+void programmingPidUpdateTask(timeUs_t currentTimeUs)
+{
+    static timeUs_t previousUpdateTimeUs;
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
+    
+    if (!pidsInitiated) {
+        programmingPidInit();
+        pidsInitiated = true;
+    }
+
+    for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        if (programmingPids(i)->enabled) {
+            const int setpoint = logicConditionGetOperandValue(programmingPids(i)->setpoint.type, programmingPids(i)->setpoint.value);
+            const int measurement = logicConditionGetOperandValue(programmingPids(i)->measurement.type, programmingPids(i)->measurement.value);
+
+            programmingPidState[i].output = navPidApply2(
+                &programmingPidState[i].controller,
+                setpoint,
+                measurement,
+                dT,
+                -1000,
+                1000,
+                PID_LIMIT_INTEGRATOR
+            );
+
+        }
+    }
+
+    previousUpdateTimeUs = currentTimeUs;
+}
+
+void programmingPidInit(void)
+{
+    for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        navPidInit(
+            &programmingPidState[i].controller,
+            programmingPids(i)->gains.P / 1000.0f,
+            programmingPids(i)->gains.I / 1000.0f,
+            programmingPids(i)->gains.D / 1000.0f,
+            programmingPids(i)->gains.FF / 1000.0f,
+            5.0f
+        );
+    }
+}
+
+int programmingPidGetOutput(uint8_t i) {
+    return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output;
+}
+
+void programmingPidReset(void)
+{
+    for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        navPidReset(&programmingPidState[i].controller);
+    }
+}
+
+#endif
--- a/src/main/programming/pid.h
+++ b/src/main/programming/pid.h
@ -0,0 +1,54 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "config/parameter_group.h"
+#include "common/time.h"
+#include "common/fp_pid.h"
+
+#include "programming/logic_condition.h"
+#include "common/axis.h"
+#include "flight/pid.h"
+
+#define MAX_PROGRAMMING_PID_COUNT 4
+
+typedef struct programmingPid_s {
+    uint8_t enabled;
+    logicOperand_t setpoint;
+    logicOperand_t measurement;
+    pid8_t gains;
+} programmingPid_t;
+
+PG_DECLARE_ARRAY(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids);
+
+typedef struct programmingPidState_s {
+    pidController_t controller;
+    float output;
+} programmingPidState_t;
+
+void programmingPidUpdateTask(timeUs_t currentTimeUs);
+void programmingPidInit(void);
+void programmingPidReset(void);
+int programmingPidGetOutput(uint8_t i);
--- a/src/main/programming/programming_task.c
+++ b/src/main/programming/programming_task.c
@ -27,7 +27,9 @@
 FILE_COMPILE_FOR_SIZE

 #include "programming/logic_condition.h"
+#include "programming/pid.h"

 void programmingFrameworkUpdateTask(timeUs_t currentTimeUs) {
+    programmingPidUpdateTask(currentTimeUs);
    logicConditionUpdateTask(currentTimeUs);
 }
--- a/src/main/rx/ghst.c
+++ b/src/main/rx/ghst.c
@ -241,7 +241,7 @@ uint8_t ghstFrameStatus(rxRuntimeConfig_t *rxRuntimeState)
 {
    UNUSED(rxRuntimeState);

-    if(serialIsIdle(serialPort)) {
+    if (serialIsIdle(serialPort)) {
        ghstIdle();
    }

--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@ -29,6 +29,8 @@
 #include "common/maths.h"
 #include "common/utils.h"

+#include "programming/logic_condition.h"
+
 #include "config/feature.h"
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@ -64,11 +66,9 @@
 #include "rx/srxl2.h"
 #include "rx/sumd.h"
 #include "rx/sumh.h"
-#include "rx/uib_rx.h"
 #include "rx/xbus.h"
 #include "rx/ghst.h"

-
 //#define DEBUG_RX_SIGNAL_LOSS

 const char rcChannelLetters[] = "AERT";
@ -330,12 +330,6 @@ void rxInit(void)
            break;
 #endif

-#ifdef USE_RX_UIB
-        case RX_TYPE_UIB:
-            rxUIBInit(rxConfig(), &rxRuntimeConfig);
-            break;
-#endif
-
 #ifdef USE_RX_SPI
        case RX_TYPE_SPI:
            if (!rxSpiInit(rxConfig(), &rxRuntimeConfig)) {
@ -723,7 +717,11 @@ uint16_t rxGetRefreshRate(void)

 int16_t rxGetChannelValue(unsigned channelNumber)
 {
-    return rcChannels[channelNumber].data;
+    if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL)) {
+        return getRcChannelOverride(channelNumber, rcChannels[channelNumber].data);
+    } else {
+        return rcChannels[channelNumber].data;
+    }
 }

 void lqTrackerReset(rxLinkQualityTracker_e * lqTracker)
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@ -65,7 +65,7 @@ typedef enum {
    RX_TYPE_SERIAL      = 2,
    RX_TYPE_MSP         = 3,
    RX_TYPE_SPI         = 4,
-    RX_TYPE_UIB         = 5
+    RX_TYPE_UNUSED_1    = 5
 } rxReceiverType_e;

 typedef enum {
--- a/src/main/rx/uib_rx.c
+++ b/src/main/rx/uib_rx.c
@ -1,126 +0,0 @@
-/*
- * 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/.
- *
- * @author Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
- */
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <string.h>
-
-#include "platform.h"
-
-#if defined(USE_UAV_INTERCONNECT) && defined(USE_RX_UIB)
-
-#include "build/build_config.h"
-#include "build/debug.h"
-
-#include "common/utils.h"
-#include "common/maths.h"
-
-#include "rx/rx.h"
-#include "rx/uib_rx.h"
-
-#include "uav_interconnect/uav_interconnect.h"
-
-#define UIB_DEVICE_ADDRESS      UIB_DEVICE_ID_RC_RECEIVER
-
-typedef struct __attribute__((packed)) {
-    uint8_t  flags;         // UIB_DATA_VALID (0x01) - link ok
-    uint8_t  rssi;
-    uint8_t  sticks[4];     // Values in range [0;255], center = 127
-    uint8_t  aux[8];        // Analog AUX channels - values in range [0;255], center = 127
-    uint16_t reserved;      // Reserved for future use
-} rcUibReceiverData_t;
-
-static rcUibReceiverData_t uibData;
-
-#define UIB_RX_MAX_CHANNEL_COUNT    16
-
-static uint16_t rxUIBReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
-{
-    UNUSED(rxRuntimeConfigPtr);
-
-    switch (chan) {
-        case 0 ... 3:
-            return scaleRange(uibData.sticks[chan], 0, 255, 1000, 2000);
-
-        case 4 ... 11:
-            return scaleRange(uibData.aux[chan - 4], 0, 255, 1000, 2000);
-
-        case 12:
-        case 13:
-        case 14:
-            return 1500;
-
-        case 15:
-            return scaleRange(uibData.rssi, 0, 255, 1000, 2000);
-
-        default:
-            return 1500;
-    }
-}
-
-static uint8_t rxUIBFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
-{
-    UNUSED(rxRuntimeConfig);
-
-    if (!uavInterconnectBusIsInitialized()) {
-        return RX_FRAME_FAILSAFE;
-    }
-
-    // If bus didn't detect the yet - report failure
-    if (!uibDetectAndActivateDevice(UIB_DEVICE_ADDRESS)) {
-        return RX_FRAME_FAILSAFE;
-    }
-
-    if (uibGetUnansweredRequests(UIB_DEVICE_ADDRESS) > 10) {         // Tolerate 200ms loss (10 packet loss)
-        return RX_FRAME_FAILSAFE;
-    }
-
-    if (uibDataAvailable(UIB_DEVICE_ADDRESS)) {
-        rcUibReceiverData_t uibDataTmp;
-        uibRead(UIB_DEVICE_ADDRESS, (uint8_t*)&uibDataTmp, sizeof(uibDataTmp));
-
-        if (!(uibDataTmp.flags & UIB_DATA_VALID))
-            return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
-
-        memcpy(&uibData, &uibDataTmp, sizeof(rcUibReceiverData_t));
-        return RX_FRAME_COMPLETE;
-    }
-    else {
-        return RX_FRAME_PENDING;
-    }
-}
-
-void rxUIBInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
-{
-    UNUSED(rxConfig);
-
-    rxRuntimeConfig->channelCount = UIB_RX_MAX_CHANNEL_COUNT;
-    rxRuntimeConfig->rxRefreshRate = 20000;
-    rxRuntimeConfig->rxSignalTimeout = DELAY_5_HZ;
-    rxRuntimeConfig->rcReadRawFn = rxUIBReadRawRC;
-    rxRuntimeConfig->rcFrameStatusFn = rxUIBFrameStatus;
-}
-#endif
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@ -101,9 +101,6 @@ typedef enum {
 #ifdef USE_OPFLOW
    TASK_OPFLOW,
 #endif
-#ifdef USE_UAV_INTERCONNECT
-    TASK_UAV_INTERCONNECT,
-#endif
 #ifdef USE_RCDEVICE
    TASK_RCDEVICE,
 #endif
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@ -56,7 +56,7 @@

 baro_t baro;                        // barometer access functions

-PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3);

 #ifdef USE_BARO
 #define BARO_HARDWARE_DEFAULT    BARO_AUTODETECT
@ -65,7 +65,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER
 #endif
 PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
    .baro_hardware = BARO_HARDWARE_DEFAULT,
-    .use_median_filtering = 0,
+    .use_median_filtering = 1,
    .baro_calibration_tolerance = 150
 );

--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@ -59,6 +59,7 @@ FILE_COMPILE_FOR_SPEED

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

 #include "io/beeper.h"
 #include "io/statusindicator.h"
@ -101,10 +102,10 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;

 #endif

-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11);

 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
-    .gyro_lpf = GYRO_LPF_42HZ,      // 42HZ value is defined for Invensense/TDK gyros
+    .gyro_lpf = GYRO_LPF_256HZ,
    .gyro_soft_lpf_hz = 60,
    .gyro_soft_lpf_type = FILTER_BIQUAD,
    .gyro_align = ALIGN_DEFAULT,
@ -116,6 +117,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
    .gyro_notch_cutoff = 1,
    .gyro_stage2_lowpass_hz = 0,
    .gyro_stage2_lowpass_type = FILTER_BIQUAD,
+    .useDynamicLpf = 0,
+    .gyroDynamicLpfMinHz = 200,
+    .gyroDynamicLpfMaxHz = 500,
+    .gyroDynamicLpfCurveExpo = 5,
    .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
    .dynamicGyroNotchQ = 120,
    .dynamicGyroNotchMinHz = 150,
@ -510,3 +515,15 @@ bool gyroSyncCheckUpdate(void)

    return gyroDev[0].intStatusFn(&gyroDev[0]);
 }
+
+void gyroUpdateDynamicLpf(float cutoffFreq) {
+    if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq);
+        }
+    } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF);
+        }
+    }
+}
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@ -70,6 +70,10 @@ typedef struct gyroConfig_s {
    uint16_t gyro_notch_cutoff;
    uint16_t gyro_stage2_lowpass_hz;
    uint8_t gyro_stage2_lowpass_type;
+    uint8_t useDynamicLpf;
+    uint16_t gyroDynamicLpfMinHz;
+    uint16_t gyroDynamicLpfMaxHz;
+    uint8_t gyroDynamicLpfCurveExpo;
    uint8_t dynamicGyroNotchRange;
    uint16_t dynamicGyroNotchQ;
    uint16_t dynamicGyroNotchMinHz;
@ -87,3 +91,4 @@ bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 bool gyroSyncCheckUpdate(void);
+void gyroUpdateDynamicLpf(float cutoffFreq);
--- a/src/main/sensors/rangefinder.c
+++ b/src/main/sensors/rangefinder.c
@ -53,8 +53,6 @@

 #include "scheduler/scheduler.h"

-#include "uav_interconnect/uav_interconnect.h"
-
 rangefinder_t rangefinder;

 #define RANGEFINDER_HARDWARE_TIMEOUT_MS         500     // Accept 500ms of non-responsive sensor, report HW failure otherwise
@ -142,15 +140,6 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
 #endif
            break;

-        case RANGEFINDER_UIB:
-#if defined(USE_RANGEFINDER_UIB)
-            if (uibRangefinderDetect(dev)) {
-                rangefinderHardware = RANGEFINDER_UIB;
-                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_UIB_TASK_PERIOD_MS));
-            }
-#endif
-            break;
-
        case RANGEFINDER_BENEWAKE:
 #if defined(USE_RANGEFINDER_BENEWAKE)
            if (virtualRangefinderDetect(dev, &rangefinderBenewakeVtable)) {
--- a/src/main/sensors/rangefinder.h
+++ b/src/main/sensors/rangefinder.h
@ -28,7 +28,7 @@ typedef enum {
    RANGEFINDER_HCSR04I2C   = 3,
    RANGEFINDER_VL53L0X     = 4,
    RANGEFINDER_MSP         = 5,
-    RANGEFINDER_UIB         = 6,
+    RANGEFINDER_UNUSED      = 6,    // Was UIB
    RANGEFINDER_BENEWAKE    = 7,
 } rangefinderType_e;

--- a/src/main/target/FF_F35_LIGHTNING/target.h
+++ b/src/main/target/FF_F35_LIGHTNING/target.h
@ -27,6 +27,8 @@
 #define BEEPER                  PA1
 #define BEEPER_INVERTED

+#define USE_DSHOT
+
 // MPU interrupt
 #define USE_EXTI
 #define GYRO_INT_EXTI            PC4
--- a/src/main/target/FLYWOOF411/CMakeLists.txt
+++ b/src/main/target/FLYWOOF411/CMakeLists.txt
@ -1 +1,2 @@
 target_stm32f411xe(FLYWOOF411)
+target_stm32f411xe(FLYWOOF411_V2)
--- a/src/main/target/FLYWOOF411/target.c
+++ b/src/main/target/FLYWOOF411/target.c
@ -26,7 +26,17 @@

 const timerHardware_t timerHardware[] = {
    DEF_TIM(TIM9, CH1, PA2,   TIM_USE_PPM,   0, 0), // PPM IN
-
+#ifdef FLYWOOF411_V2
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1),      // S1 - D(2,1)
+    DEF_TIM(TIM2,  CH2, PB3,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),      // S2 - D(1,6)
+    DEF_TIM(TIM2,  CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),      // S3 - D(1,1)
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),      // S4 - D(1,5)
+    DEF_TIM(TIM4,  CH1, PB6,  TIM_USE_ANY, 0, 0),      // SOFTSERIAL_1_TX_PIN - D(1,0)
+    DEF_TIM(TIM4,  CH2, PB7,  TIM_USE_ANY, 0, 0),      // SOFTSERIAL_1_RX_PIN - D(1,3)
+    DEF_TIM(TIM3,  CH3, PB0,  TIM_USE_ANY, 0, 0), //  RSSI_ADC_CHANNEL 1-7
+    DEF_TIM(TIM3,  CH1, PB4,  TIM_USE_ANY, 0, 0), //   1-4
+    DEF_TIM(TIM5,  CH1, PA0,  TIM_USE_LED, 0, 0), //  LED    1,2
+#else
    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 1), // S1_OUT 2,1
    DEF_TIM(TIM1, CH2, PA9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 1), // S2_OUT 2,2
    DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO,  0, 0), // S3_OUT 2,6
@ -35,7 +45,7 @@ const timerHardware_t timerHardware[] = {
    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_ANY,   0, 0), //  RSSI   1,2
    DEF_TIM(TIM5, CH4, PA3,  TIM_USE_ANY,   0, 1), //  RX2    1,0
    DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED,   0, 0), //  LED    1,5
-
+#endif
 };

 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
--- a/src/main/target/FLYWOOF411/target.h
+++ b/src/main/target/FLYWOOF411/target.h
@ -17,8 +17,13 @@

 #pragma once

+#ifdef FLYWOOF411_V2
+#define TARGET_BOARD_IDENTIFIER "FW42"
+#define USBD_PRODUCT_STRING     "FLYWOOF411V2"
+#else
 #define TARGET_BOARD_IDENTIFIER "FW41"
 #define USBD_PRODUCT_STRING     "FLYWOOF411"
+#endif

 #define LED0                    PC13

@ -52,7 +57,11 @@
 #define IMU_ICM20689_ALIGN      CW180_DEG

 #define USE_EXTI
+#ifdef FLYWOOF411_V2
+#define GYRO_INT_EXTI           PB5
+#else
 #define GYRO_INT_EXTI           PB3
+#endif
 #define USE_MPU_DATA_READY_SIGNAL

 // *************** Baro *****************************
@ -93,29 +102,51 @@
 #define USE_VCP

 #define USE_UART1
+#ifdef FLYWOOF411_V2
+#define UART1_TX_PIN            PA9
+#define UART1_RX_PIN            PA10
+#else
 #define UART1_TX_PIN            PB6
 #define UART1_RX_PIN            PB7
+#endif

 #define USE_UART2
+#ifdef FLYWOOF411_V2
+#define UART2_TX_PIN            PA2
+#else
 #define UART2_TX_PIN            NONE    //PA2
+#endif
 #define UART2_RX_PIN            PA3

 #define USE_SOFTSERIAL1
+#ifdef FLYWOOF411_V2
+#define SOFTSERIAL_1_TX_PIN     PB6     // Clash with TX2, possible to use as S.Port or VTX control
+#define SOFTSERIAL_1_RX_PIN     PB7
+#else
 #define SOFTSERIAL_1_TX_PIN     PA2     // Clash with TX2, possible to use as S.Port or VTX control
 #define SOFTSERIAL_1_RX_PIN     PA2
+#endif

 #define SERIAL_PORT_COUNT       4       // VCP, USART1, USART2, SS1

 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SBUS
+#ifdef FLYWOOF411_V2
+#define SERIALRX_UART           SERIAL_PORT_USART1
+#else
 #define SERIALRX_UART           SERIAL_PORT_USART2
-
+#endif
 // *************** ADC *****************************
 #define USE_ADC
 #define ADC_INSTANCE                    ADC1
 #define ADC_CHANNEL_1_PIN               PA1
+#ifdef FLYWOOF411_V2
+#define ADC_CHANNEL_2_PIN               PB1
+#define ADC_CHANNEL_3_PIN               PB0
+#else
 #define ADC_CHANNEL_2_PIN               PA0
 #define ADC_CHANNEL_3_PIN               PB1
+#endif

 #define CURRENT_METER_ADC_CHANNEL       ADC_CHN_1
 #define VBAT_ADC_CHANNEL                ADC_CHN_2
@ -123,8 +154,11 @@

 // *************** LED2812 ************************
 #define USE_LED_STRIP
+#ifdef FLYWOOF411_V2
+#define WS2811_PIN                      PA0
+#else
 #define WS2811_PIN                      PA15
-
+#endif
 // ***************  OTHERS *************************
 #define DEFAULT_FEATURES                (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL)

@ -138,4 +172,8 @@
 #define TARGET_IO_PORTC         0xffff
 #define TARGET_IO_PORTD        (BIT(2))

+#ifdef FLYWOOF411_V2
+#define MAX_PWM_OUTPUT_PORTS       6
+#else
 #define MAX_PWM_OUTPUT_PORTS       4
+#endif
--- a/src/main/target/KAKUTEF4/target.h
+++ b/src/main/target/KAKUTEF4/target.h
@ -70,6 +70,9 @@
 #   define BARO_I2C_BUS            BUS_I2C1
 #   define USE_BARO_MS5611
 #   define USE_BARO_BMP280
+#else // V1 does not have I2C exposed, common_post.h will pull in USE_*_MSP
+#   define USE_BARO
+#   define USE_MAG
 #endif

 #define USE_OSD
--- a/src/main/target/MATEKF405/target.c
+++ b/src/main/target/MATEKF405/target.c
@ -40,7 +40,7 @@ const timerHardware_t timerHardware[] = {
 #else
    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO, 0, 0), // S6  UP(2,5)
 #endif
-    DEF_TIM(TIM4, CH3, PB8,  TIM_USE_MC_MOTOR |                    TIM_USE_FW_MOTOR, 0, 0), // S7  D(1,7)!S5 UP(2,6)
+    DEF_TIM(TIM4, CH3, PB8,  TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7  D(1,7)!S5 UP(2,6)

    DEF_TIM(TIM5, CH3, PA2,  TIM_USE_PWM,                                            0, 0), // TX2

--- a/src/main/target/MATEKF411SE/CMakeLists.txt
+++ b/src/main/target/MATEKF411SE/CMakeLists.txt
@ -1 +1,2 @@
 target_stm32f411xe(MATEKF411SE)
+target_stm32f411xe(MATEKF411SE_FD_SFTSRL1)
--- a/src/main/target/MATEKF411SE/target.c
+++ b/src/main/target/MATEKF411SE/target.c
@ -35,7 +35,7 @@ const timerHardware_t timerHardware[] = {
    DEF_TIM(TIM5,  CH3, PA2,  TIM_USE_ANY,   0, 0), //TX2 pad -softserial_tx2

    DEF_TIM(TIM9, CH2, PA3,   TIM_USE_PPM,   0, 0), //RX2 Pad -PPM
-    DEF_TIM(TIM2, CH3, PB10,  TIM_USE_LED,   0, 0), //LED 2812   D(1,1,3)
+    DEF_TIM(TIM2, CH3, PB10,  TIM_USE_ANY,   0, 0), // softserial_rx1 - LED 2812   D(1,1,3)

 };

--- a/src/main/target/MATEKF411SE/target.h
+++ b/src/main/target/MATEKF411SE/target.h
@ -70,7 +70,11 @@

 #define USE_SOFTSERIAL1
 #define SOFTSERIAL_1_TX_PIN     PB9 // ST1 pad
+#ifdef MATEKF411SE_FD_SFTSRL1
+#define SOFTSERIAL_1_RX_PIN     PB10 // LED pad
+#else
 #define SOFTSERIAL_1_RX_PIN     PB9
+#endif

 #define USE_SOFTSERIAL2
 #define SOFTSERIAL_2_TX_PIN     PA2 // TX2 pad
@ -131,8 +135,10 @@
 #define AIRSPEED_ADC_CHANNEL        ADC_CHN_4

 // *************** LED2812 ************************
+#ifndef MATEKF411SE_FD_SFTSRL1
 #define USE_LED_STRIP
 #define WS2811_PIN                  PB10
+#endif

 // *************** PINIO ***************************
 #define USE_PINIO
--- a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt
+++ b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt
@ -0,0 +1 @@
+target_stm32f722xe(RUSH_BLADE_F7 SKIP_RELEASES)
--- a/src/main/target/RUSH_BLADE_F7/target.c
+++ b/src/main/target/RUSH_BLADE_F7/target.c
@ -0,0 +1,37 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include "platform.h"
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+
+const timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM8, CH3, PC8,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S1   UP2-1   D(2, 4, 7)
+    DEF_TIM(TIM8, CH4, PC9,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S2   UP2-1   D(2, 7, 7)
+    DEF_TIM(TIM3, CH4, PB1,     TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S6   UP1-2   D(1, 2, 5)
+    DEF_TIM(TIM3, CH3, PB0,     TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S5   UP1-2   D(1, 7, 5)
+    DEF_TIM(TIM8, CH2, PC7,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5 D(2, 3, 7)
+    DEF_TIM(TIM8, CH1, PC6,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6 D(1, 4, 5)
+
+    DEF_TIM(TIM2, CH1, PA15,    TIM_USE_LED, 0, 0),                            // LED 
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
--- a/src/main/target/RUSH_BLADE_F7/target.h
+++ b/src/main/target/RUSH_BLADE_F7/target.h
@ -0,0 +1,147 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "RBF7"
+#define USBD_PRODUCT_STRING  "RUSH_BLADE_F7"
+
+#define LED0                    PB10  //Blue   SWCLK
+// #define LED1                    PA13  //Green  SWDIO
+
+#define BEEPER                  PB2
+#define BEEPER_INVERTED
+
+// *************** SPI1 Gyro & ACC *******************
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_EXTI
+#define USE_MPU_DATA_READY_SIGNAL
+#define GYRO_INT_EXTI           PA4
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
+#define MPU6000_CS_PIN          PC4
+#define MPU6000_SPI_BUS         BUS_SPI1
+
+// *************** I2C /Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_AK8975
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
+// *************** SPI2 Flash ***********************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS          BUS_SPI2
+#define M25P16_CS_PIN           PB12
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** UART *****************************
+#define USE_VCP
+#define USB_DETECT_PIN          PC14
+#define USE_USB_DETECT
+
+#define USE_UART1
+#define UART1_TX_PIN            PA9
+#define UART1_RX_PIN            PA10
+
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            PA3
+
+#define USE_UART3
+#define UART3_TX_PIN            PC10
+#define UART3_RX_PIN            PC11
+
+#define USE_UART4
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PA1
+
+#define USE_UART5
+#define UART5_TX_PIN            PC12
+#define UART5_RX_PIN            PD2
+
+#define SERIAL_PORT_COUNT       6
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+#define DEFAULT_FEATURES   (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL)
+#define CURRENT_METER_SCALE     179
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS        10
+#define USE_OSD
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR
--- a/src/main/target/ZEEZF7/config.c
+++ b/src/main/target/ZEEZF7/config.c
@ -0,0 +1,30 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "fc/fc_msp_box.h"
+
+#include "io/piniobox.h"
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; // VTX power switcher
+    //pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
+}
--- a/src/main/target/ZEEZF7/target.h
+++ b/src/main/target/ZEEZF7/target.h
@ -44,6 +44,17 @@
 #define USE_IMU_MPU6000
 #define IMU_MPU6000_ALIGN       CW180_DEG

+// *************** I2C/Baro/Mag *********************
+// Target has no I2C but can use MSP devices, such as those provided on Mateksys MQ8-CAN/MSP
+// Which contains: GPS SAM-M8Q, Compass QMC5883L, Barometer DPS310
+// See: http://www.mateksys.com/?portfolio=m8q-can
+
+#define USE_BARO
+#define USE_BARO_DPS310
+
+#define USE_MAG
+#define USE_MAG_QMC5883
+
 // *************** Flash ****************************

 #define USE_SPI_DEVICE_1
@ -73,7 +84,7 @@
 // *************** PINIO ***************************
 #define USE_PINIO
 #define USE_PINIOBOX
-#define PINIO1_PIN              PB11
+#define PINIO1_PIN              PB11 // VTX power switcher

 // *************** UART *****************************
 #define USE_VCP
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@ -87,8 +87,6 @@
 #define USE_DYNAMIC_FILTERS
 #define USE_GYRO_KALMAN
 #define USE_EXTENDED_CMS_MENUS
-#define USE_UAV_INTERCONNECT
-#define USE_RX_UIB
 #define USE_HOTT_TEXTMODE

 // NAZA GPS support for F4+ only
--- a/src/main/telemetry/ghst.c
+++ b/src/main/telemetry/ghst.c
@ -163,7 +163,7 @@ bool checkGhstTelemetryState(void)
 // Called periodically by the scheduler
 void handleGhstTelemetry(timeUs_t currentTimeUs)
 {
-    static uint32_t ghstLastCycleTime;
+    static timeUs_t ghstLastCycleTime;

    if (!ghstTelemetryEnabled) {
        return;
--- a/src/main/uav_interconnect/uav_interconnect.h
+++ b/src/main/uav_interconnect/uav_interconnect.h
@ -1,70 +0,0 @@
-/*
- * 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/.
- *
- * @author Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
- */
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <ctype.h>
-#include <math.h>
-
-#include "platform.h"
-#include "build/build_config.h"
-#include "drivers/rangefinder/rangefinder.h"
-
-#ifdef USE_UAV_INTERCONNECT
-
-#define UIB_MAX_PACKET_SIZE 32
-
-#define UIB_DEVICE_ID_RANGEFINDER       0x12
-#define UIB_DEVICE_ID_GPS               0x13
-#define UIB_DEVICE_ID_COMPASS           0x14
-#define UIB_DEVICE_ID_RC_RECEIVER       0x80
-
-typedef enum {
-    UIB_DATA_NONE           = 0,
-    UIB_DATA_VALID          = (1 << 0),     // Data is valid
-} uibDataFlags_t;
-
-/* Bus task */
-bool uavInterconnectBusCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime);
-void uavInterconnectBusTask(timeUs_t currentTimeUs);
-void uavInterconnectBusInit(void);
-bool uavInterconnectBusIsInitialized(void);
-
-/* Bus device API */
-bool uibRegisterDevice(uint8_t devId);
-bool uibDetectAndActivateDevice(uint8_t devId);
-bool uibGetDeviceParams(uint8_t devId, uint8_t * params);
-timeUs_t uibGetPollRateUs(uint8_t devId);
-uint32_t uibGetUnansweredRequests(uint8_t devId);
-uint8_t uibDataAvailable(uint8_t devId);
-uint8_t uibRead(uint8_t devId, uint8_t * buffer, uint8_t bufSize);
-bool uibCanWrite(uint8_t devId);
-bool uibWrite(uint8_t devId, const uint8_t * buffer, uint8_t bufSize);
-
-#define RANGEFINDER_UIB_TASK_PERIOD_MS  100
-bool uibRangefinderDetect(rangefinderDev_t *dev);
-
-#endif
--- a/src/main/uav_interconnect/uav_interconnect_bus.c
+++ b/src/main/uav_interconnect/uav_interconnect_bus.c
@ -1,635 +0,0 @@
-/*
- * 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/.
- *
- * @author Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
- */
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <ctype.h>
-#include <math.h>
-#include <string.h>
-
-#include "platform.h"
-#include "build/build_config.h"
-
-#ifdef USE_UAV_INTERCONNECT
-
-#include "build/debug.h"
-
-#include "common/maths.h"
-#include "common/axis.h"
-#include "common/utils.h"
-#include "common/crc.h"
-
-#include "config/parameter_group.h"
-#include "config/parameter_group_ids.h"
-
-#include "drivers/serial.h"
-#include "drivers/time.h"
-
-#include "io/serial.h"
-
-#include "uav_interconnect/uav_interconnect.h"
-
-typedef enum {
-    UIB_COMMAND_IDENTIFY    = (0x00 << 5),  // 0x00
-    UIB_COMMAND_NOTIFY      = (0x01 << 5),  // 0x20
-    UIB_COMMAND_READ        = (0x02 << 5),  // 0x40
-    UIB_COMMAND_WRITE       = (0x03 << 5),  // 0x60
-    UIB_COMMAND_RES_1       = (0x04 << 5),  // 0x80
-    UIB_COMMAND_RES_2       = (0x05 << 5),  // 0xA0
-    UIB_COMMAND_RES_3       = (0x06 << 5),  // 0xC0
-    UIB_COMMAND_RES_4       = (0x07 << 5),  // 0xE0
-} uibCommand_e;
-
-typedef enum {
-    UIB_FLAG_HAS_READ       = (1 << 0),     // Device supports READ command (sensor)
-    UIB_FLAG_HAS_WRITE      = (1 << 1),     // Device supports WRITE command (sensor configuration or executive device)
-} uibDeviceFlags_e;
-
-#define UIB_PROTOCOL_VERSION    0x00
-#define UIB_MAX_SLOTS           16          // 32 is design maximum
-#define UIB_PORT_OPTIONS        (SERIAL_NOT_INVERTED | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_UNIDIR)
-
-#define UIB_DISCOVERY_DELAY_US  2000000 // 2 seconds from power-up to allow all devices to boot
-#define UIB_GUARD_INTERVAL_US   1000
-#define UIB_REFRESH_INTERVAL_US 200000
-
-typedef struct {
-    bool        allocated;
-    bool        activated;
-    uint8_t     deviceAddress;
-    uint16_t    deviceFlags;
-    uint8_t     devParams[4];
-    timeUs_t    pollIntervalUs;
-    timeUs_t    lastPollTimeUs;
-    uint32_t    unrepliedRequests;  // 0 - all answered, 1 - request in progress, 2 or more - device failed to answer one or more requests
-
-    uint8_t     rxDataReadySize;
-    uint8_t     txDataReadySize;
-    uint8_t     rxPacket[UIB_MAX_PACKET_SIZE];
-    uint8_t     txPacket[UIB_MAX_PACKET_SIZE];
-} uavInterconnectSlot_t;
-
-typedef enum {
-    STATE_INITIALIZE,
-    STATE_DISCOVER,
-    STATE_READY,
-} uavInterconnectState_t;
-
-typedef struct {
-    int sentCommands;
-    int discoveredDevices;
-    int failedCRC;
-    int commandTimeouts;
-} uavInterconnectStats_t;
-
-static serialPort_t *           busPort;
-static bool                     uibInitialized = false;
-
-static uavInterconnectSlot_t    slots[UIB_MAX_SLOTS];
-static timeUs_t                 slotStartTimeUs;
-static uavInterconnectState_t   busState = STATE_INITIALIZE;
-
-static int                      discoveryAddress;
-static int                      refreshSlot;
-static timeUs_t                 refreshStartTimeUs;
-
-static uint8_t                  slotDataBuffer[20]; // Max transaction length is 20 bytes
-static unsigned                 slotDataBufferCount;
-static timeUs_t                 slotLastActivityUs;
-
-// Statistics
-static uavInterconnectStats_t   uibStats;
-
-static void switchState(uavInterconnectState_t newState)
-{
-    if (busState == newState)
-        return;
-
-    busState = newState;
-}
-
-static void sendDiscover(timeUs_t currentTimeUs, uint8_t slot, uint8_t devId)
-{
-    uint8_t buf[4];
-    buf[0] = UIB_COMMAND_IDENTIFY | slot;
-    buf[1] = devId;
-    buf[2] = UIB_PROTOCOL_VERSION;
-    buf[3] = crc8_dvb_s2_update(0x00, &buf[0], 3);
-    slotStartTimeUs = currentTimeUs;
-    serialWriteBuf(busPort, buf, 4);
-    uibStats.sentCommands++;
-}
-
-static void sendNotify(timeUs_t currentTimeUs, uint8_t slot, uint8_t devId)
-{
-    uint8_t buf[4];
-    buf[0] = UIB_COMMAND_NOTIFY | slot;
-    buf[1] = devId;
-    buf[2] = UIB_PROTOCOL_VERSION;
-    buf[3] = crc8_dvb_s2_update(0x00, &buf[0], 3);
-    slotStartTimeUs = currentTimeUs;
-    serialWriteBuf(busPort, buf, 4);
-    uibStats.sentCommands++;
-}
-
-static void sendRead(timeUs_t currentTimeUs, uint8_t slot)
-{
-    uint8_t buf[2];
-    buf[0] = UIB_COMMAND_READ | slot;
-    buf[1] = crc8_dvb_s2_update(0x00, &buf[0], 1);
-    slotStartTimeUs = currentTimeUs;
-    serialWriteBuf(busPort, buf, 2);
-    uibStats.sentCommands++;
-}
-
-static void sendWrite(timeUs_t currentTimeUs, uint8_t slot, uint8_t * data, uint8_t len)
-{
-    uint8_t buf[UIB_MAX_PACKET_SIZE + 3];
-    buf[0] = UIB_COMMAND_WRITE | slot;
-    buf[1] = len;
-    memcpy(&buf[2], data, len);
-    buf[UIB_MAX_PACKET_SIZE + 2] = crc8_dvb_s2_update(0x00, &buf[0], UIB_MAX_PACKET_SIZE + 2);
-    slotStartTimeUs = currentTimeUs;
-    serialWriteBuf(busPort, buf, UIB_MAX_PACKET_SIZE + 3);
-    uibStats.sentCommands++;
-}
-
-static uavInterconnectSlot_t * findDevice(uint8_t devId)
-{
-    for (int i = 0; i < UIB_MAX_SLOTS; i++) {
-        if (slots[i].allocated && slots[i].deviceAddress == devId) {
-            return &slots[i];
-        }
-    }
-
-    return NULL;
-}
-
-static int findEmptySlot(void)
-{
-    for (int i = 0; i < UIB_MAX_SLOTS; i++) {
-        if (!slots[i].allocated)
-            return i;
-    }
-
-    return -1;
-}
-
-static void registerDeviceSlot(uint8_t slotId, uint8_t devId, uint16_t deviceFlags, uint32_t pollIntervalUs, const uint8_t * devParams)
-{
-    slots[slotId].allocated = true;
-    slots[slotId].deviceAddress = devId;
-    slots[slotId].deviceFlags = deviceFlags;
-    slots[slotId].pollIntervalUs = pollIntervalUs;
-
-    if (devParams) {
-        slots[slotId].devParams[0] = devParams[0];
-        slots[slotId].devParams[1] = devParams[1];
-        slots[slotId].devParams[2] = devParams[2];
-        slots[slotId].devParams[3] = devParams[3];
-    }
-    else {
-        slots[slotId].devParams[0] = 0;
-        slots[slotId].devParams[1] = 0;
-        slots[slotId].devParams[2] = 0;
-        slots[slotId].devParams[3] = 0;
-    }
-
-    slots[slotId].rxDataReadySize = 0;
-    slots[slotId].txDataReadySize = 0;
-    slots[slotId].lastPollTimeUs = 0;
-    slots[slotId].unrepliedRequests = 0;
-}
-
-typedef struct __attribute__((packed)) {
-    uint8_t slotAndCmd;
-    uint8_t  devId;
-    uint8_t  protoVersion;
-    uint8_t  crc1;
-    uint16_t pollIntervalMs;
-    uint16_t devFlags;
-    uint8_t  devParams[4];
-    uint8_t  crc2;
-} uibPktIdentify_t;
-
-typedef struct __attribute__((packed)) {
-    uint8_t slotAndCmd;
-    uint8_t devId;
-    uint8_t protoVersion;
-    uint8_t crc1;
-} uibPktNotify_t;
-
-typedef struct __attribute__((packed)) {
-    uint8_t slotAndCmd;
-    uint8_t crc1;
-    uint8_t size;
-    uint8_t data[0];
-} uibPktRead_t;
-
-typedef struct __attribute__((packed)) {
-    uint8_t slotAndCmd;
-    uint8_t size;
-    uint8_t data[0];
-} uibPktWrite_t;
-
-typedef union {
-    uibPktIdentify_t    ident;
-    uibPktNotify_t      notify;
-    uibPktRead_t        read;
-    uibPktWrite_t       write;
-} uibPktData_t;
-
-static void processSlot(void)
-{
-    // First byte is command / slot
-    const uint8_t cmd = slotDataBuffer[0] & 0xE0;
-    const uint8_t slot = slotDataBuffer[0] & 0x1F;
-    const uibPktData_t * pkt = (const uibPktData_t *)&slotDataBuffer[0];
-    const int lastPacketByteIndex = slotDataBufferCount - 1;
-
-    // CRC is calculated over the whole slot, including command byte(s) sent by FC. This ensures integrity of the transaction as a whole
-    switch (cmd) {
-        // Identify command (13 bytes)
-        //      FC:     IDENTIFY[1] + DevID[1] + ProtoVersion [1] + CRC1[1]
-        //      DEV:    PollInterval[2] + Flags[2] + DevParams[4] + CRC2[1]
-        case UIB_COMMAND_IDENTIFY:
-            if (slotDataBufferCount == sizeof(uibPktIdentify_t)) {
-                if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) == pkt->ident.crc2) {
-                    // CRC valid - process valid IDENTIFY slot
-                    registerDeviceSlot(slot, pkt->ident.devId, pkt->ident.devFlags, pkt->ident.pollIntervalMs * 1000, pkt->ident.devParams);
-                    uibStats.discoveredDevices++;
-                }
-                else {
-                    uibStats.failedCRC++;
-                }
-
-                // Regardless of CRC validity - discard buffer data
-                slotDataBufferCount = 0;
-            }
-            break;
-
-        // NOTIFY command (4 bytes)
-        //      FC:     IDENTIFY[1] + DevID[1] + ProtoVersion [1] + CRC1[1]
-        case UIB_COMMAND_NOTIFY:
-            if (slotDataBufferCount == sizeof(uibPktNotify_t)) {
-                // Record failed CRC. Do nothing else - NOTIFY is only sent for devices that already have a slot assigned
-                if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) != pkt->notify.crc1) {
-                    uibStats.failedCRC++;
-                }
-
-                slotDataBufferCount = 0;
-            }
-            break;
-
-        // Read command (min 4 bytes)
-        //      FC:     READ[1] + CRC1[1]
-        //      DEV:    DATA_LEN[1] + DATA[variable] + CRC2[1]
-        case UIB_COMMAND_READ:
-            if ((slotDataBufferCount >= sizeof(uibPktRead_t)) && (slotDataBufferCount == ((unsigned)pkt->read.size + 4)) && (pkt->read.size <= UIB_MAX_PACKET_SIZE)) {
-                if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) == slotDataBuffer[lastPacketByteIndex]) {
-                    // CRC valid - process valid READ slot
-                    // Check if this slot has read capability and is allocated
-                    if (slots[slot].allocated && (slots[slot].deviceFlags & UIB_FLAG_HAS_READ)) {
-                        if ((slots[slot].rxDataReadySize == 0) && (pkt->read.size > 0)) {
-                            memcpy(slots[slot].rxPacket, pkt->read.data, pkt->read.size);
-                            slots[slot].rxDataReadySize = pkt->read.size;
-                        }
-
-                        slots[slot].unrepliedRequests = 0;
-                    }
-                }
-                else {
-                    uibStats.failedCRC++;
-                }
-
-                // Regardless of CRC validity - discard buffer data
-                slotDataBufferCount = 0;
-            }
-            break;
-
-        // Write command (min 3 bytes)
-        //      FC:     WRITE[1] + DATA_LEN[1] + DATA[variable] + CRC1[1]
-        case UIB_COMMAND_WRITE:
-            if ((slotDataBufferCount >= sizeof(uibPktWrite_t)) && (slotDataBufferCount == ((unsigned)pkt->write.size + 3)) && (pkt->write.size <= UIB_MAX_PACKET_SIZE)) {
-                // Keep track of failed CRC events
-                if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) != slotDataBuffer[lastPacketByteIndex]) {
-                    uibStats.failedCRC++;
-                }
-
-                // Regardless of CRC validity - discard buffer data
-                slotDataBufferCount = 0;
-            }
-            break;
-
-        default:
-            break;
-    }
-}
-
-static void processScheduledTransactions(timeUs_t currentTimeUs)
-{
-    int slotPrio = 0x100;
-    int slotId = -1;
-
-    // First - find device with highest priority that has the READ capability and is scheduled for READ
-    for (int i = 0; i < UIB_MAX_SLOTS; i++) {
-        // Only do scheduled READs on allocated and active slots
-        if (!(slots[i].allocated && slots[i].activated))
-            continue;
-
-        if ((slots[i].deviceFlags & UIB_FLAG_HAS_READ) && ((currentTimeUs - slots[i].lastPollTimeUs) >= slots[i].pollIntervalUs) && (slotPrio > slots[i].deviceAddress)) {
-            slotId = i;
-            slotPrio = slots[i].deviceAddress;
-        }
-    }
-
-    // READ command
-    if (slotId >= 0) {
-        sendRead(currentTimeUs, slotId);
-        slots[slotId].unrepliedRequests++;
-        slots[slotId].lastPollTimeUs = currentTimeUs;
-        return;
-    }
-
-    // No READ command executed - check if we have data to WRITE
-    slotPrio = 0x100;
-    slotId = -1;
-
-    for (int i = 0; i < UIB_MAX_SLOTS; i++) {
-        // Only do WRITEs on allocated and active slots
-        if (!(slots[i].allocated && slots[i].activated))
-            continue;
-
-        if (slots[i].txDataReadySize && (slots[i].deviceFlags & UIB_FLAG_HAS_WRITE) && (slotPrio > slots[i].deviceAddress)) {
-            slotId = i;
-            slotPrio = slots[i].deviceAddress;
-        }
-    }
-
-    // WRITE command
-    if (slotId >= 0) {
-        sendWrite(currentTimeUs, slotId, slots[slotId].txPacket, slots[slotId].txDataReadySize);
-        slots[slotId].unrepliedRequests++;
-        slots[slotId].txDataReadySize = 0;
-        return;
-    }
-
-    // Neither READ nor WRITE command are queued - issue IDENTIFY once in a while
-    if ((currentTimeUs - refreshStartTimeUs) > UIB_REFRESH_INTERVAL_US) {
-        // Send notifications for allocated slots
-        if (slots[refreshSlot].allocated) {
-            sendNotify(currentTimeUs, refreshSlot, slots[refreshSlot].deviceAddress);
-            refreshStartTimeUs = currentTimeUs;
-        }
-
-        if (++refreshSlot >= UIB_MAX_SLOTS) {
-            refreshSlot = 0;
-        }
-    }
-}
-
-static bool canSendNewRequest(timeUs_t currentTimeUs)
-{
-    return ((currentTimeUs - slotLastActivityUs) >= UIB_GUARD_INTERVAL_US);
-}
-
-void uavInterconnectBusTask(timeUs_t currentTimeUs)
-{
-    if (!uibInitialized)
-        return;
-
-    // Receive bytes to the buffer
-    bool hasNewBytes = false;
-    while (serialRxBytesWaiting(busPort) > 0) {
-        uint8_t c = serialRead(busPort);
-        if (slotDataBufferCount < (int)(sizeof(slotDataBuffer) / sizeof(slotDataBuffer[0]))) {
-            slotDataBuffer[slotDataBufferCount++] = c;
-            hasNewBytes = true;
-        }
-
-        slotLastActivityUs = currentTimeUs;
-    }
-
-    // Flush receive buffer if guard interval elapsed
-    if ((currentTimeUs - slotLastActivityUs) >= UIB_GUARD_INTERVAL_US && slotDataBufferCount > 0) {
-        uibStats.commandTimeouts++;
-        slotDataBufferCount = 0;
-    }
-
-    // If we have new bytes - process packet
-    if (hasNewBytes && slotDataBufferCount >= 4) {  // minimum transaction length is 4 bytes - no point in processing something smaller
-        processSlot();
-    }
-
-    // Process request scheduling - we can initiate another slot if guard interval has elapsed and slot interval has elapsed as well
-    if (canSendNewRequest(currentTimeUs)) {
-        // We get here only if we can send requests - no timeout checking should be done beyond this point
-        switch (busState) {
-            case STATE_INITIALIZE:
-                if ((currentTimeUs - slotStartTimeUs) > UIB_DISCOVERY_DELAY_US) {
-                    discoveryAddress = 0;
-                    switchState(STATE_DISCOVER);
-                }
-                break;
-
-            case STATE_DISCOVER:
-                {
-                    int discoverySlot = findEmptySlot();
-                    if (discoverySlot >= 0) {
-                        sendDiscover(currentTimeUs, discoverySlot, discoveryAddress);
-                        if (discoveryAddress == 0xFF) {
-                            // All addresses have been polled
-                            switchState(STATE_READY);
-                        }
-                        else {
-                            // Query next address and stick here
-                            discoveryAddress++;
-                        }
-                    }
-                    else {
-                        // All slots are allocated - can't discover more devices
-                        refreshSlot = 0;
-                        refreshStartTimeUs = currentTimeUs;
-                        switchState(STATE_READY);
-                    }
-                }
-                break;
-
-            case STATE_READY:
-                // Bus ready - process scheduled transfers
-                processScheduledTransactions(currentTimeUs);
-                break;
-        }
-    }
-}
-
-void uavInterconnectBusInit(void)
-{
-    for (int i = 0; i < UIB_MAX_SLOTS; i++) {
-        slots[i].allocated = false;
-        slots[i].activated = false;
-    }
-
-    serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_UAV_INTERCONNECT);
-    if (!portConfig)
-        return;
-
-    baudRate_e baudRateIndex = portConfig->peripheral_baudrateIndex;
-    busPort = openSerialPort(portConfig->identifier, FUNCTION_UAV_INTERCONNECT, NULL, NULL, baudRates[baudRateIndex], MODE_RXTX, UIB_PORT_OPTIONS);
-    if (!busPort)
-        return;
-
-    slotStartTimeUs = 0;
-    uibInitialized = true;
-}
-
-bool uavInterconnectBusIsInitialized(void)
-{
-    return uibInitialized;
-}
-
-bool uibDetectAndActivateDevice(uint8_t devId)
-{
-    uavInterconnectSlot_t * slot = findDevice(devId);
-    if (slot == NULL)
-        return false;
-
-    /* We have discovered device in out registry and code is asking to access it - activate device */
-    slot->activated = true;
-
-    return slot->allocated;
-}
-
-bool uibRegisterDevice(uint8_t devId)
-{
-    int slotId = findEmptySlot();
-    if (slotId >= 0) {
-        registerDeviceSlot(slotId, devId, UIB_FLAG_HAS_WRITE, 0, NULL);
-        return uibDetectAndActivateDevice(devId);
-    }
-
-    return false;
-}
-
-bool uibGetDeviceParams(uint8_t devId, uint8_t * params)
-{
-    uavInterconnectSlot_t * slot = findDevice(devId);
-    if (slot == NULL || params == NULL)
-        return false;
-
-    params[0] = slot->devParams[0];
-    params[1] = slot->devParams[1];
-    params[2] = slot->devParams[2];
-    params[3] = slot->devParams[3];
-    return true;
-}
-
-timeUs_t uibGetPollRateUs(uint8_t devId)
-{
-    uavInterconnectSlot_t * slot = findDevice(devId);
-    if (slot == NULL)
-        return 0;
-
-    return slot->pollIntervalUs;
-}
-
-uint32_t uibGetUnansweredRequests(uint8_t devId)
-{
-    uavInterconnectSlot_t * slot = findDevice(devId);
-    if (slot == NULL)
-        return 0;
-
-    return (slot->unrepliedRequests < 2) ? 0 : slot->unrepliedRequests - 1;
-}
-
-uint8_t uibDataAvailable(uint8_t devId)
-{
-    uavInterconnectSlot_t * slot = findDevice(devId);
-    if (slot == NULL)
-        return 0;
-
-    return slot->rxDataReadySize;
-}
-
-uint8_t uibRead(uint8_t devId, uint8_t * buffer, uint8_t bufSize)
-{
-    uavInterconnectSlot_t * slot = findDevice(devId);
-    if (slot == NULL)
-        return 0;
-
-    // If no READ capability - fail
-    if (!(slot->deviceFlags & UIB_FLAG_HAS_READ))
-        return false;
-
-    // If no data ready - fail
-    if (!slot->rxDataReadySize)
-        return 0;
-
-    uint8_t bytes = slot->rxDataReadySize;
-    memcpy(buffer, slot->rxPacket, MIN(bytes, bufSize));
-    slot->rxDataReadySize = 0;
-
-    return bytes;
-}
-
-static bool slotCanWrite(const uavInterconnectSlot_t * slot)
-{
-    // If no WRITE capability - fail
-    if (!(slot->deviceFlags & UIB_FLAG_HAS_WRITE))
-        return false;
-
-    // If we have unsent data in the buffer - fail
-    if (slot->txDataReadySize > 0)
-        return false;
-
-    return true;
-}
-
-bool uibCanWrite(uint8_t devId)
-{
-    // No device available
-    uavInterconnectSlot_t * slot = findDevice(devId);
-    if (slot == NULL)
-        return false;
-
-    return slotCanWrite(slot);
-}
-
-bool uibWrite(uint8_t devId, const uint8_t * buffer, uint8_t len)
-{
-    uavInterconnectSlot_t * slot = findDevice(devId);
-    if (slot == NULL)
-        return false;
-
-    if (slotCanWrite(slot)) {
-        memcpy(slot->txPacket, buffer, len);
-        slot->txDataReadySize = len;
-        return true;
-    }
-
-    return false;
-}
-#endif
--- a/src/main/uav_interconnect/uav_interconnect_rangefinder.c
+++ b/src/main/uav_interconnect/uav_interconnect_rangefinder.c
@ -1,121 +0,0 @@
-/*
- * 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/.
- *
- * @author Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
- */
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <ctype.h>
-#include <math.h>
-#include <string.h>
-
-#include "platform.h"
-#include "build/build_config.h"
-#include "build/debug.h"
-
-#include "common/maths.h"
-#include "common/axis.h"
-#include "common/utils.h"
-
-#include "config/parameter_group.h"
-#include "config/parameter_group_ids.h"
-
-#ifdef USE_RANGEFINDER_UIB
-#include "drivers/rangefinder/rangefinder.h"
-#include "uav_interconnect/uav_interconnect.h"
-
-#define UIB_DEVICE_ADDRESS                      UIB_DEVICE_ID_RANGEFINDER
-
-#define RANGEFINDER_MAX_RANGE_CM                1000
-#define RANGEFINDER_DETECTION_CONE_DECIDEGREES  450
-
-typedef struct __attribute__((packed)) {
-    uint8_t flags;
-    uint16_t distanceCm;
-} rangefinderData_t;
-
-static int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
-static rangefinderData_t uibData;
-
-static void uibRangefinderInit(rangefinderDev_t *dev)
-{
-    UNUSED(dev);
-}
-
-static void uibRangefinderUpdate(rangefinderDev_t *dev)
-{
-    UNUSED(dev);
-
-    if (!uavInterconnectBusIsInitialized()) {
-        lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
-        return;
-    }
-
-    // If bus didn't detect the yet - report failure
-    if (!uibDetectAndActivateDevice(UIB_DEVICE_ADDRESS)) {
-        lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
-        return;
-    }
-
-    // If device is not responding - report failure
-    if (uibGetUnansweredRequests(UIB_DEVICE_ADDRESS) > 0) {
-        lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
-        return;
-    }
-
-    if (uibDataAvailable(UIB_DEVICE_ADDRESS)) {
-        uibRead(UIB_DEVICE_ADDRESS, (uint8_t*)&uibData, sizeof(uibData));
-
-        if (uibData.flags & UIB_DATA_VALID) {
-            lastCalculatedDistance = uibData.distanceCm;
-            dev->delayMs = uibGetPollRateUs(UIB_DEVICE_ADDRESS) / 1000;
-        }
-        else {
-            lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
-        }
-    }
-}
-
-static int32_t uibRangefinderGetDistance(rangefinderDev_t *dev)
-{
-    UNUSED(dev);
-    return lastCalculatedDistance;
-}
-
-bool uibRangefinderDetect(rangefinderDev_t *dev)
-{
-    // This always succeed
-    dev->delayMs = RANGEFINDER_UIB_TASK_PERIOD_MS;
-    dev->maxRangeCm = RANGEFINDER_MAX_RANGE_CM;
-    dev->detectionConeDeciDegrees = RANGEFINDER_DETECTION_CONE_DECIDEGREES;
-    dev->detectionConeExtendedDeciDegrees = RANGEFINDER_DETECTION_CONE_DECIDEGREES;
-
-    dev->init = &uibRangefinderInit;
-    dev->update = &uibRangefinderUpdate;
-    dev->read = &uibRangefinderGetDistance;
-
-    return true;
-}
-
-#endif
--- a/src/test/unit/sensor_gyro_unittest.cc
+++ b/src/test/unit/sensor_gyro_unittest.cc
@ -37,6 +37,8 @@ extern "C" {
    #include "sensors/gyro.h"
    #include "sensors/acceleration.h"
    #include "sensors/sensors.h"
+    #include "fc/rc_controls.h"
+    #include "flight/mixer.h"

    extern zeroCalibrationVector_t gyroCalibration;
    extern gyroDev_t gyroDev[];
				`@ -0,0 +1 @@`
				`target_stm32f722xe(RUSH_BLADE_F7 SKIP_RELEASES)`