diff --git a/.dockerignore b/.dockerignore
new file mode 100644
index 0000000000..f6718b745c
--- /dev/null
+++ b/.dockerignore
@@ -0,0 +1,2 @@
+.vagrant/
+obj/
diff --git a/.github/ISSUE_TEMPLATE/Bug_report.md b/.github/ISSUE_TEMPLATE/Bug_report.md
index 6f6952875f..eb7f126be9 100644
--- a/.github/ISSUE_TEMPLATE/Bug_report.md
+++ b/.github/ISSUE_TEMPLATE/Bug_report.md
@@ -7,6 +7,19 @@ assignees: ''
 
 ---
 
+**PLEASE MAKE SURE YOU READ AND UNDERSTAND THE SOCIAL MEDIA SUPPORT CHANNELS. QUESTIONS ABOUT FLASHING, CONFIGURING, PILOTING MAY BE CLOSED WITHOUT FURTHER INTERACTION.**
+
+* [Telegram channel](https://t.me/INAVFlight)
+* [Facebook group](https://www.facebook.com/groups/INAVOfficial)
+* [RC Groups thread](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-%28navigation-rewrite%29-project)
+
+**Please double-check that nobody reported the issue before by using search in this bug tracker.**
+
+**PLEASE DELETE THE TEXT ABOVE AFTER READING AND UNDERSTANDING IT**
+
+****
+
+
 ## Current Behavior
 <!-- If applicable, add screenshots, videos and blackbox logs to help explain your problem. -->
 
diff --git a/.github/ISSUE_TEMPLATE/Question.md b/.github/ISSUE_TEMPLATE/Question.md
deleted file mode 100644
index 2850f8b410..0000000000
--- a/.github/ISSUE_TEMPLATE/Question.md
+++ /dev/null
@@ -1,23 +0,0 @@
----
-name: "❓Question"
-about: Have a question?
-title: ''
-labels: ''
-assignees: ''
-
----
-
-For immediate help, just ask your question on one of the following platforms:
-
-* [Telegram channel](https://t.me/INAVFlight)
-* [Facebook group](https://www.facebook.com/groups/INAVOfficial)
-* [RC Groups thread](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-%28navigation-rewrite%29-project)
-
-You can also read public documentations or watch video tutorials:
-
-* [Official documentation](https://github.com/iNavFlight/inav/tree/master/docs)
-* [Official Wiki](https://github.com/iNavFlight/inav/wiki)
-* [Video series by Painless360](https://www.youtube.com/playlist?list=PLYsWjANuAm4qdXEGFSeUhOZ10-H8YTSnH)
-* [Video series by Paweł Spychalski](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLloACrA6f1_daCjhqY2x0fB)
-* [How to setup INAV on a flying wing](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLn_lCX7UB2OPLzR4w5G4el3)
-* [How to setup INAV on a 5" quad](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLl9M6Vm-ZNLMCr6_9yNVHQG)
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
new file mode 100644
index 0000000000..35bfbdd88e
--- /dev/null
+++ b/.github/workflows/ci.yml
@@ -0,0 +1,68 @@
+name: Build firmware
+# Don't enable CI on push, just on PR. If you
+# are working on the main repo and want to trigger
+# a CI build submit a draft PR.
+on: pull_request
+
+jobs:
+  build:
+    runs-on: ubuntu-18.04
+    strategy:
+        matrix:
+          start: [0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100]
+          count: [10]
+
+    steps:
+      - uses: actions/checkout@v2
+      - name: Setup environment
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          echo "::set-env name=TARGETS::$(./src/utils/build-targets.sh -n -s ${{ matrix.start }} -c ${{ matrix.count }})"
+          echo "::set-env name=BUILD_SUFFIX::${BUILD_SUFFIX}"
+          echo "::set-env name=BUILD_NAME::inav-$(make print_version)-${BUILD_SUFFIX}"
+          echo "::set-env name=IS_LAST_JOB::$([ $(expr ${{ strategy.job-index }} + 1) = ${{ strategy.job-total }} ] && echo yes)"
+      - name: Ensure all targets will be tested
+        if: ${{ env.IS_LAST_JOB }}
+        run: |
+          UNTESTED=$(./src/utils/build-targets.sh -n -s $(expr ${{ matrix.start }} + ${{ matrix.count }}) -c 10000)
+          if ! [ -z "${UNTESTED}" ]; then
+            echo "Untested targets: ${UNTESTED}" >&2
+            exit 1
+          fi
+      - uses: actions/cache@v1
+        with:
+          path: downloads
+          key: ${{ runner.os }}-downloads-${{ hashFiles('Makefile') }}-${{ hashFiles('**/make/*.mk')}}
+      - name: Install ARM toolchain
+        run: make arm_sdk_install
+      - name: Build targets (${{ matrix.start }})
+        if: ${{ env.TARGETS }}
+        run: ./src/utils/build-targets.sh -s ${{ matrix.start }} -c ${{ matrix.count }} -S ${{ env.BUILD_SUFFIX }}
+      - name: Upload artifacts
+        if: ${{ env.TARGETS }}
+        uses: actions/upload-artifact@v2-preview
+        with:
+          name: ${{ env.BUILD_NAME }}.zip
+          path: ./obj/dist/*.hex
+
+  test:
+    needs: [build]
+    runs-on: ubuntu-18.04
+    steps:
+      - uses: actions/checkout@v2
+      - uses: actions/cache@v1
+        with:
+          path: downloads
+          key: ${{ runner.os }}-downloads-${{ hashFiles('Makefile') }}-${{ hashFiles('**/make/*.mk')}}
+      - name: Install ARM toolchain
+        run: make arm_sdk_install
+      - name: Run Tests
+        run: make test
diff --git a/Dockerfile b/Dockerfile
index de2b3aa084..746733bbeb 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -4,9 +4,9 @@ FROM ubuntu:bionic
 VOLUME /home/src/
 WORKDIR /home/src/
 ARG TOOLCHAIN_VERSION_SHORT
-ENV TOOLCHAIN_VERSION_SHORT ${TOOLCHAIN_VERSION_SHORT:-"8-2018q4"}
+ENV TOOLCHAIN_VERSION_SHORT ${TOOLCHAIN_VERSION_SHORT:-"9-2019q4"}
 ARG TOOLCHAIN_VERSION_LONG
-ENV TOOLCHAIN_VERSION_LONG ${TOOLCHAIN_VERSION_LONG:-"8-2018-q4-major"}
+ENV TOOLCHAIN_VERSION_LONG ${TOOLCHAIN_VERSION_LONG:-"9-2019-q4-major"}
 
 # Essentials
 RUN mkdir -p /home/src && \
@@ -14,10 +14,13 @@ RUN mkdir -p /home/src && \
     apt-get install -y software-properties-common ruby make git gcc wget curl bzip2
 
 # Toolchain
-RUN wget -P /tmp "https://developer.arm.com/-/media/Files/downloads/gnu-rm/$TOOLCHAIN_VERSION_SHORT/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-linux.tar.bz2"
+RUN wget -P /tmp "https://developer.arm.com/-/media/Files/downloads/gnu-rm/$TOOLCHAIN_VERSION_SHORT/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-x86_64-linux.tar.bz2"
 RUN mkdir -p /opt && \
 	cd /opt && \
-    tar xvjf "/tmp/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-linux.tar.bz2" -C /opt && \
+    tar xvjf "/tmp/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-x86_64-linux.tar.bz2" -C /opt && \
 	chmod -R -w "/opt/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG"
 
 ENV PATH="/opt/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG/bin:$PATH"
+
+RUN useradd inav
+USER inav
diff --git a/Makefile b/Makefile
index f19b1b0938..aa20153cca 100644
--- a/Makefile
+++ b/Makefile
@@ -64,8 +64,6 @@ endif
 # Things that need to be maintained as the source changes
 #
 
-FORKNAME      = inav
-
 # Working directories
 SRC_DIR         := $(ROOT)/src/main
 OBJECT_DIR      := $(ROOT)/obj/main
@@ -85,17 +83,9 @@ MHZ_VALUE      ?=
 # used for turning on features like VCP and SDCARD
 FEATURES        =
 
+include $(ROOT)/make/version.mk
 include $(ROOT)/make/targets.mk
 
-REVISION = $(shell git rev-parse --short HEAD)
-
-FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' )
-FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk '{print $$3}' )
-FC_VER_PATCH := $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' )
-
-FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH)
-FC_VER_SUFFIX ?=
-
 BUILD_DATE = $(shell date +%Y%m%d)
 
 # Search path for sources
@@ -174,12 +164,15 @@ SIZE        = $(ARM_SDK_PREFIX)size
 # Tool options.
 #
 
+# Save original CFLAGS before modifying them, so we don't
+# add them twice when calling this Makefile recursively
+# for each target
+SAVED_CFLAGS	:= $(CFLAGS)
+
 ifeq ($(DEBUG),GDB)
-OPTIMIZE    = -O0
-LTO_FLAGS   = $(OPTIMIZE)
+LTO_FLAGS   = 
 else
-OPTIMIZE    = -Os
-LTO_FLAGS   = -flto -fuse-linker-plugin $(OPTIMIZE)
+LTO_FLAGS   = -flto -fuse-linker-plugin
 endif
 
 ifneq ($(SEMIHOSTING),)
@@ -252,9 +245,6 @@ CPPCHECK        = cppcheck $(CSOURCES) --enable=all --platform=unix64 \
 # Things we will build
 #
 TARGET_BIN	:= $(BIN_DIR)/$(FORKNAME)_$(FC_VER)
-ifneq ($(FC_VER_SUFFIX),)
-    TARGET_BIN	:= $(TARGET_BIN)-$(FC_VER_SUFFIX)
-endif
 TARGET_BIN	:= $(TARGET_BIN)_$(TARGET)
 ifneq ($(BUILD_SUFFIX),)
     TARGET_BIN	:= $(TARGET_BIN)_$(BUILD_SUFFIX)
@@ -301,16 +291,38 @@ $(TARGET_ELF): $(TARGET_OBJS)
 	$(V1) $(CROSS_CC) -o $@ $(filter %.o, $^) $(LDFLAGS)
 	$(V0) $(SIZE) $(TARGET_ELF)
 
+OPTIMIZE_FLAG_SPEED := "-Os"
+OPTIMIZE_FLAG_SIZE := "-Os"
+OPTIMIZE_FLAG_NORMAL := "-Os"
+
+ifneq ($(TARGET_MCU_GROUP), STM32F3)
+	OPTIMIZE_FLAG_SPEED := "-Ofast"
+	OPTIMIZE_FLAG_SIZE := "-Os"
+	OPTIMIZE_FLAG_NORMAL := "-O2"
+endif
+
+define compile_file
+	echo "%% $(1) $(2) $<" "$(STDOUT)" && \
+	$(CROSS_CC) -c -o $@ $(CFLAGS) $(2) $<
+endef
+
 # Compile
 $(TARGET_OBJ_DIR)/%.o: %.c
 	$(V1) mkdir -p $(dir $@)
-	$(V1) echo %% $(notdir $<) "$(STDOUT)"
-	$(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $<
+
+	$(V1) $(if $(findstring $<,$(SIZE_OPTIMISED_SRC)), \
+		$(call compile_file,(size),$(OPTIMIZE_FLAG_SIZE)) \
+	, \
+		$(if $(findstring $<,$(SPEED_OPTIMISED_SRC)), \
+			$(call compile_file,(speed),$(OPTIMIZE_FLAG_SPEED)) \
+		, \
+			$(call compile_file,(normal),$(OPTIMIZE_FLAG_NORMAL)) \
+		) \
+	)
 ifeq ($(GENERATE_ASM), 1)
 	$(V1) $(CROSS_CC) -S -fverbose-asm -Wa,-aslh -o $(patsubst %.o,%.txt.S,$@) -g $(ASM_CFLAGS) $<
 endif
 
-
 # Assemble
 $(TARGET_OBJ_DIR)/%.o: %.s
 	$(V1) mkdir -p $(dir $@)
@@ -340,7 +352,7 @@ release: $(RELEASE_TARGETS)
 $(VALID_TARGETS):
 	$(V0) echo "" && \
 	echo "Building $@" && \
-	$(MAKE) -j 8 TARGET=$@ && \
+	CFLAGS=$(SAVED_CFLAGS) $(MAKE) -j 8 TARGET=$@ && \
 	echo "Building $@ succeeded."
 
 ## clean             : clean up all temporary / machine-generated files
diff --git a/README.md b/README.md
index 73d8f3ce6a..aa996c6788 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,9 @@
 # INAV - navigation capable flight controller
 
+## F3 based flight controllers
+
+> STM32 F3 flight controllers like Omnibus F3 or SP Racing F3 are deprecated and soon they will reach the end of support in INAV. If you are still using F3 boards, please migrate to F4 or F7.
+
 ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
 ![Travis CI status](https://travis-ci.org/iNavFlight/inav.svg?branch=master)
 
diff --git a/docs/Autotune - fixedwing.md b/docs/Autotune - fixedwing.md
index e3d9c11966..3579e8e58d 100755
--- a/docs/Autotune - fixedwing.md	
+++ b/docs/Autotune - fixedwing.md	
@@ -30,7 +30,7 @@ For most hobby-sized airplanes roll/pitch rate limits should be in range 70-120
 
 Other things to check:
 
-* It's highly recommended that you fly in PASTHROUGH and trim your servo midpoints for stable flight
+* It's highly recommended that you fly in MANUAL and trim your servo midpoints for stable flight
 * Make sure you have center of gravity according to manual to your aircraft
 * Check that your failsafe activates correctly (test on the ground with propeller off for safety)
 
@@ -54,6 +54,6 @@ The more you fly the better it will get. Let autotune analyze how your airplane
 
 ## Completing the tune
 
-Once you have tuned reasonable PIFF parameters with AUTOTUNE you should complete the tune by switching out of AUTOTUNE to ANGLE or PASTHROUGH and landing the airplane.
+Once you have tuned reasonable PIFF parameters with AUTOTUNE you should complete the tune by switching out of AUTOTUNE to ANGLE or MANUAL and landing the airplane.
 
 Note that AUTOTUNE mode doesn't automatically save parameters to EEPROM. You need to disarm and issue a [stick command](Controls.md) to save configuration parameters.
diff --git a/docs/Battery.md b/docs/Battery.md
index 1b5e23ebd3..f7277b264a 100644
--- a/docs/Battery.md
+++ b/docs/Battery.md
@@ -26,7 +26,7 @@ On the first battery connection is always advisable to use a current limiter dev
 
 ### Sparky
 
-See the [Sparky board chapter](Board - Sparky.md).
+See the [Sparky board chapter](Board%20-%20Sparky.md).
 
 ## Voltage measurement
 
diff --git a/docs/Board - MatekF722-SE.md b/docs/Board - MatekF722-SE.md
index 6a14d973fa..8fcf3177fe 100644
--- a/docs/Board - MatekF722-SE.md	
+++ b/docs/Board - MatekF722-SE.md	
@@ -22,7 +22,7 @@ Full details on the MATEKSYS F722-SE can be found on the Matek Website: [mateksy
 
 ### Integrated PDB Specs
 
-* *Input:* 6-36v (3-8S LiPo) w/TVS protection
+* *Input:* 6-36v (2-8S LiPo) w/TVS protection
 * *ESC Pads:* Rated 4x30A per ESC pad set (4x46A burst)
 * Voltage Regulators:
   * *5v BEC:* 2A continuous load (3A burst)
diff --git a/docs/Cli.md b/docs/Cli.md
index cfb8e3d065..19a5f0aa9c 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -152,7 +152,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  switch_disarm_delay | 250 | Delay before disarming when requested by switch (ms) [0-1000] |
 |  small_angle  | 25 | If the aircraft tilt angle exceed this value the copter will refuse to arm.  |
 |  reboot_character  | 82 | Special character used to trigger reboot |
-|  gps_provider  | UBLOX | Which GPS protocol to be used |
+|  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_dyn_model  | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. |
 |  gps_auto_config  | ON | Enable automatic configuration of UBlox GPS receivers. |
@@ -163,7 +163,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  inav_gravity_cal_tolerance  | 5 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. |
 |  inav_use_gps_velned  | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. |
 |  inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) |
-|  inav_reset_home | EACH_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM |
+|  inav_reset_home | FIRST_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM |
 |  inav_max_surface_altitude  | 200 | Max allowed altitude for surface following mode. [cm] |
 |  inav_w_z_baro_p  | 0.350 | Weight of barometer measurements in estimated altitude and climb rate |
 |  inav_w_z_gps_p  | 0.200 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes |
@@ -215,6 +215,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  nav_fw_climb_angle  | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
 |  nav_fw_dive_angle  | 15 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
 |  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) |
+|  nav_fw_control_smoothness | 0 | How smoothly the autopilot controls the airplane to correct the navigation error |
 |  nav_fw_loiter_radius  | 5000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] |
 |  nav_fw_launch_velocity  | 300 | Forward velocity threshold for swing-launch detection [cm/s] |
 |  nav_fw_launch_accel  | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |
@@ -231,6 +232,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. |
 |  nav_fw_land_dive_angle  | 2 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees |
 |  nav_fw_cruise_yaw_rate  | 20 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]|
+| nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing |
 |  serialrx_provider  | SPEK1024 | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. |
 |  serialrx_halfduplex  | OFF | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire |
 |  serialrx_inverted     | OFF | Reverse the serial inversion of the  serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). |
@@ -291,6 +293,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  alt_hold_deadband  | 50 | Defines the deadband of throttle during alt_hold [r/c points] |
 |  yaw_motor_direction  | 1 | Use if you need to inverse yaw motor direction. |
 |  tri_unarmed_servo  | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
+|  servo_protocol  | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) |
 |  servo_lpf_hz  | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] |
 |  servo_center_pulse  | 1500 | Servo midpoint |
 |  servo_pwm_rate  | 50 | Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. |
@@ -339,12 +342,15 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  osd_gforce_axis_alarm_max | 5  | Value above which the OSD axis g force indicators will blink (g) |
 |  osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_imu_temp_alarm_max | 600 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
+|  osd_esc_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
+|  osd_esc_temp_alarm_max | 900 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_baro_temp_alarm_max | 600 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. |
 |  osd_estimations_wind_compensation  | ON | Use wind estimation for remaining flight time/distance estimation |
 |  osd_failsafe_switch_layout  | OFF | If enabled the OSD automatically switches to the first layout during failsafe |
 |  osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` |
+|  osd_ahi_style | DEFAULT | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. |
 |  display_force_sw_blink  | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON |
 |  magzero_x  | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. |
 |  magzero_y  | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. |
@@ -369,6 +375,9 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  nav_fw_pos_xy_p  | 75 | P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH |
 |  nav_fw_pos_xy_i  | 5 | I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero |
 |  nav_fw_pos_xy_d  | 8 | D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero |
+|  nav_fw_pos_hdg_p  | 60 | P gain of heading PID controller. (Fixedwing) |
+|  nav_fw_pos_hdg_i  | 0 | I gain of heading trajectory PID controller. (Fixedwing) |
+|  nav_fw_pos_hdg_d  | 0 | D gain of heading trajectory PID controller. (Fixedwing) |
 |  nav_mc_heading_p  | 60 | P gain of Heading Hold controller (Multirotor) |
 |  nav_fw_heading_p  | 60 | P gain of Heading Hold controller (Fixedwing) |
 |  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. |
diff --git a/docs/Global Functions.md b/docs/Global Functions.md
index 53910caa85..f74d772b65 100644
--- a/docs/Global Functions.md	
+++ b/docs/Global Functions.md	
@@ -26,6 +26,8 @@ _Global Functions_ (abbr. GF) are a mechanism allowing to override certain fligh
 | 5             | INVERT_PITCH                  | Inverts PITCH axis input for PID/PIFF controller  |
 | 6             | INVERT_YAW                    | Inverts YAW axis input for PID/PIFF controller |
 | 7             | OVERRIDE_THROTTLE             | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
+| 8             | SET_VTX_BAND           | Sets VTX band. Accepted values are `1-5` |
+| 8             | SET_VTX_CHANNEL           | Sets VTX channel. Accepted values are `1-8` |
 
 ## Flags
 
diff --git a/docs/Logic Conditions.md b/docs/Logic Conditions.md
index ac8ae4d8fd..c04fa06353 100644
--- a/docs/Logic Conditions.md	
+++ b/docs/Logic Conditions.md	
@@ -13,6 +13,7 @@ Logic conditions can be edited using INAV Configurator user interface, of via CL
 
 * `<rule>` - ID of Logic Condition rule
 * `<enabled>` - `0` evaluates as disabled, `1` evaluates as enabled
+* `<activatorId>` - the ID of _LogicCondition_ used to activate this _Condition_. _Logic Condition_ will be evaluated only then Activator evaluates as `true`. `-1` evaluates as `true`
 * `<operation>` - See `Operations` paragraph
 * `<operand A type>` - See `Operands` paragraph
 * `<operand A value>` - See `Operands` paragraph
diff --git a/docs/Mixer.md b/docs/Mixer.md
index 575aba098c..1e3cc1369e 100644
--- a/docs/Mixer.md
+++ b/docs/Mixer.md
@@ -82,7 +82,7 @@ Each servo mixing rule has the following parameters:
 | 26 | Stabilized PITCH-        | Clipped between -1000 and 0 |
 | 27 | Stabilized YAW+          | Clipped between 0 and 1000 |
 | 28 | Stabilized YAW-          | Clipped between -1000 and 0 |
-| 29 | One                      | Constant value of 500 |
+| 29 | MAX                      | Constant value of 500 |
 
 The `smix reset` command removes all the existing motor mixing rules.
 
diff --git a/docs/Navigation.md b/docs/Navigation.md
index f56ae80489..1b31da1289 100755
--- a/docs/Navigation.md
+++ b/docs/Navigation.md
@@ -1,6 +1,6 @@
 # Navigation
 
-Navigation system in INAV is responsible for assisting the pilot allowing altitude and position hold, return-to-home and waypoint flight.
+The navigation system in INAV is responsible for assisting the pilot allowing altitude and position hold, return-to-home and waypoint flight.
 
 ## NAV ALTHOLD mode - altitude hold
 
@@ -23,7 +23,7 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co
 ## NAV POSHOLD mode - position hold
 
 Position hold requires GPS, accelerometer and compass sensors. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated.
-When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). Can be activated together with ALTHOLD to get a full 3D position hold. Heading hold in this mode is assumed and activated automatically.
+When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From inav 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically.
 
 ### CLI parameters affecting ALTHOLD mode:
 * *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own.
@@ -36,7 +36,7 @@ PID meaning:
 
 ## NAV RTH - return to home mode
 
-Home for RTH is position, where copter was armed. RTH requires accelerometer, compass and GPS sensors.
+Home for RTH is the position where vehicle was armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. RTH requires accelerometer, compass and GPS sensors.
 
 If barometer is NOT present, RTH will fly directly to home, altitude control here is up to pilot.
 
@@ -54,22 +54,57 @@ When deciding what altitude to maintain, RTH has 4 different modes of operation
 
 `wp load` - Load list of waypoints from EEPROM to FC.
 
-`wp <n> <action> <lat> <lon> <alt> <p1> <flag>` - Set parameters of waypoint with index `<n>`.
+`wp <n> <action> <lat> <lon> <alt> <p1> <p2> <p3> <flag>` - Set parameters of waypoint with index `<n>`. Note that prior to inav 2.5, the `p2` and `p3` parameters were not required. From 2.5, inav will accept either version but always saves and lists the later full version.
 
 Parameters:
 
-  * `<action>` - When 0 then waypoint is not used, when 1 then it is normal waypoint, when 4 then it is RTH.
-  
+  * `<action>` - The action to be taken at the WP. The following are enumerations are available in inav 2.5 and later:
+      *  0 - Unused / Unassigned
+      *  1 - WAYPOINT
+      *  3 - POSHOLD_TIME
+      *  4 - RTH
+      *  6 - JUMP
+      *  8 - LAND
+
   * `<lat>` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789).
-  
+
   * `<lon>` - Longitude.
-  
+
   * `<alt>` - Altitude in cm.
-  
-  * `<p1>` - For a "RTH waypoint" p1 > 0 alow landing. For a normal waypoint it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed.
-  
+
+  * `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number).
+
+  * `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For JUMP it is the number of iterations of the JUMP.
+
+  * `<p3>` - Reserved for future use. If `p2` is provided, then `p3` is also required.
+
   * `<flag>` - Last waypoint must have set `flag` to 165 (0xA5), otherwise 0.
-  
+
 `wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`).
 
-`wp reset` - Resets the list, sets the waypoints number to 0 and mark it as invalid (but doesn't delete the waypoints).
+`wp reset` - Resets the list, sets the number of waypoints to 0 and marks the list as invalid (but doesn't delete the waypoint definitions).
+
+### `wp` example
+
+```
+# wp load
+
+# wp
+#wp 11 valid
+wp 0 1 543533193 -45179273 3500 0 0 0 0
+wp 1 1 543535723 -45193913 3500 0 0 0 0
+wp 2 1 543544541 -45196617 5000 0 0 0 0
+wp 3 1 543546578 -45186895 5000 0 0 0 0
+wp 4 6 0 0 0 1 2 0 0
+wp 5 1 543546688 -45176009 3500 0 0 0 0
+wp 6 1 543541225 -45172673 3500 0 0 0 0
+wp 7 6 0 0 0 0 1 0 0
+wp 8 3 543531383 -45190405 3500 45 0 0 0
+wp 9 1 543548470 -45182104 3500 0 0 0 0
+wp 10 8 543540521 -45178091 6000 0 0 0 165
+wp 11 0 0 0 0 0 0 0 0
+...
+wp 59 0 0 0 0 0 0 0 0
+```
+
+Note that the `wp` CLI command shows waypoint list indices, while the MW-XML definition used by mwp, ezgui and the configurator use WP numbers.
diff --git a/docs/Rx.md b/docs/Rx.md
index 951c1b4c91..be8d896a32 100644
--- a/docs/Rx.md
+++ b/docs/Rx.md
@@ -83,6 +83,22 @@ The bug prevents use of all 16 channels.  Upgrade to the latest OpenTX version t
 without the fix you are limited to 8 channels regardless of the CH1-16/D16 settings.
 
 
+### F.Port
+
+F.Port is a protocol running on async serial allowing 16 controls channels and telemetry on a single UART.
+
+Supported receivers include FrSky R-XSR, X4R, X4R-SB, XSR, XSR-M, R9M Slim, R9M Slim+, R9 Mini. For ACCST receivers you need to flash the corresponding firmware for it to output F.Port. For ACCESS receivers the protocol output from the receiver can be switched between S.Bus and F.Port from the model's setup page in the RX options.
+
+#### Connection
+
+Just connect the S.Port wire from the receiver to the TX pad of a free UART on your flight controller
+
+#### Configuration
+
+```
+set serialrx_inverted = true
+set serialrx_halfduplex = true
+```
 
 ### XBUS
 
diff --git a/docs/Telemetry.md b/docs/Telemetry.md
index 72c593c7e7..8c8b406ccf 100644
--- a/docs/Telemetry.md
+++ b/docs/Telemetry.md
@@ -120,7 +120,7 @@ The following sensors are transmitted
 * **0420** : distance to GPS home fix, in meters
 * **0430** : if `frsky_pitch_roll = ON` set this will be pitch degrees*10
 * **0440** : if `frsky_pitch_roll = ON` set this will be roll degrees*10
-
+* **0450** : 'Flight Path Vector' or 'Course over ground' in degrees*10
 ### Compatible SmartPort/INAV telemetry flight status
 
 To quickly and easily monitor these SmartPort sensors and flight modes, install [iNav LuaTelemetry](https://github.com/iNavFlight/LuaTelemetry) to your Taranis Q X7, X9D, X9D+ or X9E transmitter.
diff --git a/docs/development/Building in Docker.md b/docs/development/Building in Docker.md
index fee0b31acf..8fc0b3f704 100755
--- a/docs/development/Building in Docker.md	
+++ b/docs/development/Building in Docker.md	
@@ -1,24 +1,34 @@
 # Building with Docker
 
-Building in Docker is remarkably easy.
+Building with [Docker](https://www.docker.com/) is remarkably easy: an isolated container will hold all the needed compilation tools so that they won't interfere with your system and you won't need to install and manage them by yourself. You'll only need to have Docker itself [installed](https://docs.docker.com/install/).
 
-## Build a container with toolchain
+The first time that you'll run a build it will take a little more time than following executions since it will be building its base image first. Once this initial process is completed, the firmware will be always built immediately.
+
+If you want to start from scratch - _even if that's rarely needed_ - delete the `inav-build` image on your system (`docker image rm inav-build`).
+
+## Linux
+
+In the repo's root, run:
 
 ```
-docker build -t inav-build .
+./build.sh <TARGET>
 ```
 
-## Building firmware with Docker on Ubuntu
+Where `<TARGET>` must be replaced with the name of the target that you want to build. For example:
 
-Build specified target
 ```
-sh build.sh SPRACINGF3
+./build.sh MATEKF405SE
 ```
 
-## Building firmware with Docker on Windows 10
+## Windows 10
 
-Path in Docker on Windows works in a _strange_ way, so you have to provide full path for `docker run` command. For example:
+Docker on Windows requires full paths for mounting volumes in `docker run` commands. For example: `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav` .
 
-`docker run --rm -v //c/Users/pspyc/Documents/Projects/inav:/home/src/ inav-build make TARGET=AIRBOTF4`
+You'll have to manually execute the same steps that the build script does:
 
-So, `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav`
+1. `docker build -t inav-build .`
+   + This step is only needed the first time.
+2. `docker run --rm -v <PATH_TO_REPO>:/home/src/ inav-build make TARGET=<TARGET>`
+   + Where `<PATH_TO_REPO>` must be replaced with the absolute path of where you cloned this repo (see above), and `<TARGET>` with the name of the target that you want to build.
+
+Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details.
\ No newline at end of file
diff --git a/docs/development/Building in Ubuntu.md b/docs/development/Building in Ubuntu.md
deleted file mode 100644
index a76eec0c85..0000000000
--- a/docs/development/Building in Ubuntu.md	
+++ /dev/null
@@ -1,111 +0,0 @@
-# Building in Ubuntu
-
-iNav requires a reasonably modern `gcc-arm-none-eabi` compiler. As a consequence of the long term support options in Ubuntu, it is possible that the distribution compiler will be too old to build iNav firmware. For example, Ubuntu 16.04 LTS ships with version 4.9.3 which cannot compile contemporary iNav.
-
-As of August 2018, the recommendation for Ubuntu releases is:
-
-| Release | Compiler Source |
-| ------- | --------------- |
-| 16.04 or earlier | Use the 'official' PPA |
-| 17.10 | Use the 'official' PPA as the distro compiler (5.4) *may* be too old |
-| 18.04 | Use the 'official' PPA, as the distro compiler (6.3) was broken when last tested |
-
-For Ubuntu derivatives (ElementaryOS, Mint etc.), please check the distro provided version, and if it's lower than 6, use the PPA.
-
-e.g. ElementaryOS
-
-```
-$ apt show gcc-arm-none-eabi
-...
-Version: 15:4.9.3+svn231177-1
-```
-
-This 4.9.3 and will not build iNav, so we need the PPA.
-
-## Installer commands
-
-Older versions of Debian / Ubuntu and derivatives use the `apt-get` command; newer versions use `apt`. Use the appropriate command for your release.
-
-# Prerequisites
-
-Regardless of the cross-compiler version, it is necessary to install some other tools:
-
-```
-sudo apt install git
-sudo apt install gcc
-sudo apt install ruby
-```
-
-A ruby release of at least 2 or later is recommended, if your release only provides 1.9, it is necessary to install a later version:
-
-```
-sudo apt-get remove ruby
-sudo apt-add-repository ppa:brightbox/ruby-ng
-sudo apt-get update
-sudo apt-get install ruby2.4 ruby2.4-dev
-```
-
-# Using the Distro compiler
-
-In case Ubuntu ever provides a modern compiler (as of August 2018, not recommended):
-
-```
-sudo apt install gcc-arm-none-eabi
-```
-
-# Using the PPA compiler
-
-The PPA compiler is recommended for all cases:
-
-```
-sudo apt-get remove binutils-arm-none-eabi gcc-arm-none-eabi
-sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa
-sudo apt-get update
-sudo apt-get install gcc-arm-embedded
-```
-
-After these steps, on Ubuntu 16.04, (at least of March 2018) you should now have:
-
-```
-$ arm-none-eabi-gcc -dumpversion
-7.2.1
-```
-
-Which is more than adequate for our purposes.
-
-# Building from the Github repository
-
-After the ARM cross-compiler toolchain from is installed, you should be able to build from source.
-
-```
-mkdir src
-cd src
-git clone https://github.com/iNavFlight/inav.git
-cd inav
-make
-```
-
-If you have a github account with registered ssh key you can replace the `git clone` command with  `git clone git@github.com:iNavFlight/inav.git` instead of the https link.
-
-By default, this builds the REVO target, to build another target, specify the target name to the make command, for example:
-```
-make TARGET=MATEKF405
-```
-The resultant hex file are in the `obj/` directory.
-
-You can use the INAV Configurator to flash the local ```obj/inav_TARGET.hex``` file, or use `stm32flash` or `dfu-util` directly from the command line.
-
-[msp-tool](https://github.com/fiam/msp-tool) and [flash.sh](https://github.com/stronnag/mwptools/blob/master/docs/MiscTools.asciidoc#flashsh) provide / describe 3rd party helper tools for command line flashing.
-
-# Updating and rebuilding
-
-In order to update your local firmware build:
-
-* Navigate to the local iNav repository
-* Use the following steps to pull the latest changes and rebuild your local version of iNav:
-
-```
-cd src/inav
-git pull
-make TARGET=<TARGET>
-```
diff --git a/docs/development/serial_printf_debugging.md b/docs/development/serial_printf_debugging.md
index 0d6bc015ab..7aa4e919a0 100644
--- a/docs/development/serial_printf_debugging.md
+++ b/docs/development/serial_printf_debugging.md
@@ -74,7 +74,7 @@ If the CLI `log_topics` is non-zero, then all topics matching the mask will be d
 
 ## Code usage
 
-A set of macros `LOG_S()` (log system) through `LOG_D()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
+A set of macros `LOG_E()` (log error) through `LOG_D()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
 
 ```
 //  LOG_D(topic, fmt, ...)
diff --git a/docs/policies/CODE_OF_CONDUCT.md b/docs/policies/CODE_OF_CONDUCT.md
index 9da50525b6..60c654f66a 100644
--- a/docs/policies/CODE_OF_CONDUCT.md
+++ b/docs/policies/CODE_OF_CONDUCT.md
@@ -2,35 +2,38 @@
 
 ## Our Pledge
 
-In the interest of fostering an open and welcoming environment, we as contributors and maintainers pledge to making participation in our project and our community a harassment-free experience for everyone, regardless of age, body size, disability, ethnicity, gender identity and expression, level of experience, nationality, personal appearance, race, religion, or sexual identity and orientation.
+We as members, contributors, and leaders pledge to make participation in our community a harassment-free experience for everyone, regardless of age, body size, visible or invisible disability, ethnicity, sex characteristics, gender identity and expression, level of experience, education, socio-economic status, nationality, personal appearance, race, religion, or sexual identity and orientation.
+
+We pledge to act and interact in ways that contribute to an open, welcoming, diverse, inclusive, and healthy community.
 
 ## Our Standards
 
-Examples of behavior that contributes to creating a positive environment include:
+Examples of behavior that contributes to creating a positive environment include but are not limited to:
 
-* Using welcoming and inclusive language
-* Being respectful of differing viewpoints and experiences
-* Gracefully accepting constructive criticism
-* Focusing on what is best for the community
-* Showing empathy towards other community members
+* Demonstrating empathy and kindness toward other people
+* Being respectful of differing opinions, viewpoints, and experiences
+* Giving and gracefully accepting constructive feedback
+* Accepting responsibility and apologizing to those affected by our mistakes, and learning from the experience
+* Focusing on what is best not just for us as individuals, but for the overall community
 
-Examples of unacceptable behavior by participants include:
+Examples of unacceptable behavior include but are not limited to:
 
-* The use of sexualized language or imagery and unwelcome sexual attention or advances
-* Trolling, insulting/derogatory comments, and personal or political attacks
+* The use of sexualized language or imagery, and sexual attention or advances of any kind
+* Trolling, insulting or derogatory comments, and personal or political attacks
 * Public or private harassment
-* Publishing others' private information, such as a physical or electronic address, without explicit permission
+* Publishing others’ private information, such as a physical or email address, without their explicit permission
 * Other conduct which could reasonably be considered inappropriate in a professional setting
+* Acts of unacceptable behavior on other public resources outside of but in relation to this project
 
 ## Our Responsibilities
 
-Project maintainers are responsible for clarifying the standards of acceptable behavior and are expected to take appropriate and fair corrective action in response to any instances of unacceptable behavior.
+Project maintainers and community leaders are responsible for clarifying and enforcing our standards of acceptable behavior and will take appropriate and fair corrective action in response to any behavior that they deem inappropriate, threatening, offensive, or harmful.
 
-Project maintainers have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, or to ban temporarily or permanently any contributor for other behaviors that they deem inappropriate, threatening, offensive, or harmful.
+Project maintainers and community leaders have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, and will communicate reasons for moderation decisions when appropriate.
 
 ## Scope
 
-This Code of Conduct applies both within project spaces and in public spaces when an individual is representing the project or its community. Examples of representing a project or community include using an official project e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event. Representation of a project may be further defined and clarified by project maintainers.
+This Code of Conduct applies within all community spaces, and also applies when an individual is officially representing the community in public spaces. Examples of representing our community include using an official e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event.
 
 ## Enforcement
 
@@ -40,7 +43,7 @@ Project maintainers who do not follow or enforce the Code of Conduct in good fai
 
 ## Attribution
 
-This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, available at [http://contributor-covenant.org/version/1/4][version]
+This Code of Conduct is adapted from the [Contributor Covenant][homepage],
+version 2.0, available at https://www.contributor-covenant.org/version/2/0/code_of_conduct.html.
 
 [homepage]: http://contributor-covenant.org
-[version]: http://contributor-covenant.org/version/1/4/
\ No newline at end of file
diff --git a/docs/policies/NEW_HARDWARE_POLICY.md b/docs/policies/NEW_HARDWARE_POLICY.md
index 6d38333a60..1e97413480 100644
--- a/docs/policies/NEW_HARDWARE_POLICY.md
+++ b/docs/policies/NEW_HARDWARE_POLICY.md
@@ -1,4 +1,4 @@
-# INAV New Hardware policies
+# INAV Hardware Policy
 
 ## General
 
@@ -10,33 +10,42 @@ To prevent explosive growth of different target and feature count and ensure rea
 
 "Hardware" - physical hardware requiring support from firmware side.
 
+"Requester" - manufacturer or community member seeking addition of new hardware or target
+
 ## New target additions
 
 New targets are accepted into INAV code if any of the following conditions is satisfied:
 
-1. Board manufacturer provides specs, schematics and production samples are provided to at least two core developers. In this case the new target becomes part of official INAV release.
+1. Requester is a manufacturer of the hardware or an affiliated community member. Requester provides specs, schematics and production samples are provided to at least two core developers. In this case the new target becomes part of official INAV release.
 
-2. Community member or board manufacturer provides board samples to at least one core developer and the target already exists in official Cleanflight or Betaflight firmware. In this case the new target may or may not become part of official release based on the decision made by core developers.
+2. Requester is a community member not affiliated with a manufacturer of the hardware. Requester provides board samples to at least one core developer and the target is already supported in official Cleanflight or Betaflight firmware. In this case the new target may or may not become part of official release based on the decision made by core developers.
 
-# New hardware support
+3. The new target must meet the following minimal requirements:
+
+  * On-board sensors include at least the IMU (gyroscope + accelerometer)
+  * At least 2 hardware serial ports are available with both TX and RX pins
+  * At least 512K of firmware flash memory and at least of 64K of RAM available
+  * At least one I2C bus broken out (SCL and SDA pins) and not shared with other functions 
+
+## New hardware support
 
 For the hardware which does not require a new target, but still require support from the firmware side the following rules apply:
 
-1. Hardware manufacturer provides specs and production samples for the unsupported hardware to at least two core developers.
+1. Requester is a manufacturer of the hardware or an affiliated community member. Requester provides specs and production samples for the unsupported hardware to at least two core developers.
 
-2. Community member or hardware manufacturer provides hardware samples to at least one core developer and the firmware support already exists in official Cleanflight or Betaflight firmware.
+2. Requester is a community member not affiliated with a manufacturer of the hardware. Requester provides hardware samples to at least one core developer and the firmware support already exists in official Cleanflight or Betaflight firmware.
 
-# Using own hardware exception
+## Using own hardware exception
 
-If one of the core developers has the hardware in possession they may opt in and submit support for it anyway. In this case the support is not official and is generally not included in official releases.
+If one of the core developers has the hardware in possession they may opt in and submit support for it anyway. In this case the support is not official and may not be included in official releases.
 
-# Providing samples to developers
+## Providing samples to developers
 
-1. Hardware provided to the developers would be considered a donation to the INAV project. Under no circumstances developer will be obliged to return the hardware.
+1. Hardware provided to the developers would be considered a donation to the INAV project. Under no circumstances developer will be obliged to return the hardware or pay for it.
 
-2. Manufacturer or community member providing the hardware bears all the costs of the hardware, delivery and VAT/customs duties. Hardware manufacturer also assumes the responsibility to provide up to date specs, documentation and firmware (if applicable).
+2. Requester bears all the costs of the hardware, delivery and VAT/customs duties. Hardware manufacturer also assumes the responsibility to provide up to date specs, documentation and firmware (if applicable).
 
-3. Before sending samples the providing party should reach out to developers and agree on specific terms of implementing support for the unsupported hardware. Developers may place additional requirements on a case by case basis and at their sole discretion.
+3. Before sending samples the Requester should reach out to developers and agree on specific terms of implementing support for the unsupported hardware. Developers may place additional requirements on a case by case basis and at their sole discretion.
 
 4. The new target and new hardware policies do not apply in the following cases. Developers may still chose to apply the "own hardware exception" at their own discretion.
 
@@ -46,4 +55,16 @@ If one of the core developers has the hardware in possession they may opt in and
 
 5. It's advised to provide more than one sample to avoid issues with damaged or dead on arrival hardware.
 
+## Implementing support for the new target or hardware
 
+1. Pull request to add the new target or hardware may be authored by a contributor outside INAV team or by one of the core developers. 
+
+2. There is no obligation to accept a pull request made by an outside contributor. INAV team reserves the right to reject that pull request and re-implement the support or take that pull request as a baseline and amend it.
+
+3. INAV team reserves the right to reject the new target or hardware or remove the support for an unforeseen reason including, but not limited to violation of [INAV Code of Conduct](CODE_OF_CONDUCT.md) by the manufacturer or an affiliated outside contributor.
+
+## Guidelines on contacting the team
+
+1. Requester is advised to open a feature request to add support for certain hardware to INAV by following [this link](https://github.com/iNavFlight/inav/issues/new/choose)
+
+2. After opening a feature request, Requester is advised to contact the core development team by [email](mailto:coredev@inavflight.com) mentioning the open feature request and communicate with developer team via email to arrange hardware and specifications delivery.
diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk
index 02e2548074..3c740a564b 100644
--- a/make/mcu/STM32F4.mk
+++ b/make/mcu/STM32F4.mk
@@ -29,7 +29,6 @@ EXCLUDES        = stm32f4xx_crc.c \
                   stm32f4xx_cryp_aes.c \
                   stm32f4xx_hash_md5.c \
                   stm32f4xx_cryp_des.c \
-                  stm32f4xx_rtc.c \
                   stm32f4xx_hash.c \
                   stm32f4xx_dbgmcu.c \
                   stm32f4xx_cryp_tdes.c \
diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk
index a7f23aa58d..342a8cbf95 100644
--- a/make/mcu/STM32F7.mk
+++ b/make/mcu/STM32F7.mk
@@ -37,8 +37,6 @@ EXCLUDES        = stm32f7xx_hal_can.c \
                   stm32f7xx_hal_nor.c \
                   stm32f7xx_hal_qspi.c \
                   stm32f7xx_hal_rng.c \
-                  stm32f7xx_hal_rtc.c \
-                  stm32f7xx_hal_rtc_ex.c \
                   stm32f7xx_hal_sai.c \
                   stm32f7xx_hal_sai_ex.c \
                   stm32f7xx_hal_sd.c \
diff --git a/make/release.mk b/make/release.mk
index bc7c18e83e..45271457e3 100644
--- a/make/release.mk
+++ b/make/release.mk
@@ -1,6 +1,6 @@
 RELEASE_TARGETS = AIRHEROF3 AIRHEROF3_QUAD
 
-RELEASE_TARGETS += LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405
+RELEASE_TARGETS += LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405 DALRCF722DUAL
 RELEASE_TARGETS += QUANTON REVO SPARKY2 PIKOBLX CLRACINGF4AIR CLRACINGF4AIRV2 PIXRACER BEEROTORF4 ANYFCF7 ANYFCF7_EXTERNAL_BARO
 RELEASE_TARGETS += ALIENFLIGHTNGF7
 
@@ -10,7 +10,7 @@ RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KAKUTEF7HDV
 
 RELEASE_TARGETS += SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO SPRACINGF7DUAL
 
-RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2
+RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 AIRBOTF7 OMNIBUSF7NANOV7
 RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4PROV2
 RELEASE_TARGETS += OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS
 RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7
diff --git a/make/source.mk b/make/source.mk
index 288219253e..cb16ae755e 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -16,6 +16,7 @@ COMMON_SRC = \
             common/log.c \
             common/logic_condition.c \
             common/global_functions.c \
+            common/global_variables.c \
             common/maths.c \
             common/memory.c \
             common/olc.c \
@@ -43,8 +44,10 @@ COMMON_SRC = \
             drivers/exti.c \
             drivers/io.c \
             drivers/io_pca9685.c \
+            drivers/irlock.c \
             drivers/light_led.c \
             drivers/osd.c \
+            drivers/persistent.c \
             drivers/resource.c \
             drivers/rx_nrf24l01.c \
             drivers/rx_spi.c \
@@ -62,7 +65,9 @@ COMMON_SRC = \
             drivers/sound_beeper.c \
             drivers/stack_check.c \
             drivers/system.c \
+            drivers/time.c \
             drivers/timer.c \
+            drivers/usb_msc.c \
             drivers/lights_io.c \
             drivers/1-wire.c \
             drivers/1-wire/ds_crc.c \
@@ -101,6 +106,7 @@ COMMON_SRC = \
             flight/dynamic_gyro_notch.c \
             io/beeper.c \
             io/esc_serialshot.c \
+            io/servo_sbus.c \
             io/frsky_osd.c \
             io/osd_dji_hd.c \
             io/lights.c \
@@ -127,6 +133,7 @@ COMMON_SRC = \
             rx/nrf24_syma.c \
             rx/nrf24_v202.c \
             rx/pwm.c \
+            rx/frsky_crc.c \
             rx/rx.c \
             rx/rx_spi.c \
             rx/sbus.c \
@@ -139,13 +146,14 @@ COMMON_SRC = \
             scheduler/scheduler.c \
             sensors/acceleration.c \
             sensors/battery.c \
-            sensors/temperature.c \
             sensors/boardalignment.c \
             sensors/compass.c \
             sensors/diagnostics.c \
             sensors/gyro.c \
             sensors/initialisation.c \
             sensors/esc_sensor.c \
+            sensors/irlock.c \
+            sensors/temperature.c \
             uav_interconnect/uav_interconnect_bus.c \
             uav_interconnect/uav_interconnect_rangefinder.c \
             blackbox/blackbox.c \
@@ -200,6 +208,7 @@ COMMON_SRC = \
             navigation/navigation_pos_estimator.c \
             navigation/navigation_pos_estimator_agl.c \
             navigation/navigation_pos_estimator_flow.c \
+            navigation/navigation_rover_boat.c \
             sensors/barometer.c \
             sensors/pitotmeter.c \
             sensors/rangefinder.c \
@@ -234,6 +243,7 @@ TARGET_SRC   := $(filter-out $(MCU_EXCLUDES), $(TARGET_SRC))
 
 ifneq ($(filter ONBOARDFLASH,$(FEATURES)),)
 TARGET_SRC += \
+            drivers/flash.c \
             drivers/flash_m25p16.c \
             io/flashfs.c \
             $(MSC_SRC)
@@ -268,7 +278,6 @@ TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_f32.c
 TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
 TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c
 TARGET_SRC += $(DSP_LIB)/Source/CommonTables/arm_common_tables.c
-
 TARGET_SRC += $(DSP_LIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
 TARGET_SRC += $(DSP_LIB)/Source/StatisticsFunctions/arm_max_f32.c
 
@@ -277,3 +286,14 @@ endif
 
 # Search path and source files for the ST stdperiph library
 VPATH        := $(VPATH):$(STDPERIPH_DIR)/src
+
+SIZE_OPTIMISED_SRC := ""
+SPEED_OPTIMISED_SRC := ""
+ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
+# SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
+            # ./src/main/common/filter.c \
+
+# SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
+            # ./src/main/common/maths.c \
+
+endif #!F3
diff --git a/make/system-id.mk b/make/system-id.mk
index c224b824e7..c4c2d81729 100644
--- a/make/system-id.mk
+++ b/make/system-id.mk
@@ -18,6 +18,13 @@ ifeq ($(UNAME), Linux)
   LINUX := 1
 endif
 
+# FreeBSD
+ifeq ($(UNAME), FreeBSD)
+  OSFAMILY := linux
+  LINUX := 1
+endif
+
+
 # Mac OSX
 ifeq ($(UNAME), Darwin)
   OSFAMILY := macosx
diff --git a/make/tools.mk b/make/tools.mk
index 28e1953b61..dd56b7a286 100644
--- a/make/tools.mk
+++ b/make/tools.mk
@@ -32,9 +32,20 @@ ifdef WINDOWS
 endif
 
 ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))
+ARM_SDK_DOWNLOAD_PATH := $(DL_DIR)/$(ARM_SDK_FILE)
 
 SDK_INSTALL_MARKER := $(ARM_SDK_DIR)/bin/arm-none-eabi-gcc-$(GCC_REQUIRED_VERSION)
 
+.PHONY: arm_sdk_print_filename
+
+arm_sdk_print_filename:
+	@echo $(ARM_SDK_FILE)
+
+.PHONY: arm_sdk_print_download_path
+
+arm_sdk_print_download_path:
+	@echo $(ARM_SDK_DOWNLOAD_PATH)
+
 arm_sdk_install: | $(TOOLS_DIR)
 
 arm_sdk_install: arm_sdk_download $(SDK_INSTALL_MARKER)
@@ -42,17 +53,17 @@ arm_sdk_install: arm_sdk_download $(SDK_INSTALL_MARKER)
 $(SDK_INSTALL_MARKER):
 ifneq ($(OSFAMILY), windows)
     # binary only release so just extract it
-	$(V1) tar -C $(TOOLS_DIR) -xjf "$(DL_DIR)/$(ARM_SDK_FILE)"
+	$(V1) tar -C $(TOOLS_DIR) -xjf "$(ARM_SDK_DOWNLOAD_PATH)"
 else
-	$(V1) unzip -q -d $(ARM_SDK_DIR) "$(DL_DIR)/$(ARM_SDK_FILE)"
+	$(V1) unzip -q -d $(ARM_SDK_DIR) "$(ARM_SDK_DOWNLOAD_PATH)"
 endif
 
 .PHONY: arm_sdk_download
 arm_sdk_download: | $(DL_DIR)
-arm_sdk_download: $(DL_DIR)/$(ARM_SDK_FILE)
-$(DL_DIR)/$(ARM_SDK_FILE):
+arm_sdk_download: $(ARM_SDK_DOWNLOAD_PATH)
+$(ARM_SDK_DOWNLOAD_PATH):
     # download the source only if it's newer than what we already have
-	$(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" -z "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)"
+	$(V1) curl -L -k -o "$(ARM_SDK_DOWNLOAD_PATH)" -z "$(ARM_SDK_DOWNLOAD_PATH)" "$(ARM_SDK_URL)"
 
 
 ## arm_sdk_clean     : Uninstall Arm SDK
@@ -69,7 +80,7 @@ arm_sdk_clean:
 
 ifeq ($(shell [ -d "$(ARM_SDK_DIR)" ] && echo "exists"), exists)
   ARM_SDK_PREFIX := $(ARM_SDK_DIR)/bin/arm-none-eabi-
-else ifeq (,$(findstring _install,$(MAKECMDGOALS)))
+else ifeq (,$(findstring print_,$(MAKECMDGOALS))$(filter targets,$(MAKECMDGOALS))$(findstring arm_sdk,$(MAKECMDGOALS)))
   GCC_VERSION = $(shell arm-none-eabi-gcc -dumpversion)
   ifeq ($(GCC_VERSION),)
     $(error **ERROR** arm-none-eabi-gcc not in the PATH. Run 'make arm_sdk_install' to install automatically in the tools folder of this repo)
@@ -79,4 +90,4 @@ else ifeq (,$(findstring _install,$(MAKECMDGOALS)))
 
   # ARM tookchain is in the path, and the version is what's required.
   ARM_SDK_PREFIX ?= arm-none-eabi-
-endif
\ No newline at end of file
+endif
diff --git a/make/version.mk b/make/version.mk
new file mode 100644
index 0000000000..729979023a
--- /dev/null
+++ b/make/version.mk
@@ -0,0 +1,18 @@
+FORKNAME		:= inav
+
+FC_VER_MAJOR	:= $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' )
+FC_VER_MINOR	:= $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk '{print $$3}' )
+FC_VER_PATCH	:= $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' )
+FC_VER 			:= $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH)
+
+FC_VER_SUFFIX	?=
+ifneq ($(FC_VER_SUFFIX),)
+	FC_VER += -$(FC_VER_SUFFIX)
+endif
+
+REVISION 		:= $(shell git rev-parse --short HEAD)
+
+.PHONY: print_version
+
+print_version:
+	@echo $(FC_VER)
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 4095224c87..d023e9c287 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -77,6 +77,7 @@
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
+#include "sensors/esc_sensor.h"
 #include "flight/wind_estimator.h"
 #include "sensors/temperature.h"
 
@@ -390,6 +391,10 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
     {"sens6Temp",             -1, SIGNED,   PREDICT(0),      ENCODING(SIGNED_VB)},
     {"sens7Temp",             -1, SIGNED,   PREDICT(0),      ENCODING(SIGNED_VB)},
 #endif
+#ifdef USE_ESC_SENSOR
+    {"escRPM",                -1, UNSIGNED, PREDICT(0),             ENCODING(UNSIGNED_VB)},
+    {"escTemperature",        -1, SIGNED,   PREDICT(PREVIOUS),      ENCODING(SIGNED_VB)},
+#endif
 };
 
 typedef enum BlackboxState {
@@ -498,6 +503,10 @@ typedef struct blackboxSlowState_s {
 #ifdef USE_TEMPERATURE_SENSOR
     int16_t tempSensorTemperature[MAX_TEMP_SENSORS];
 #endif
+#ifdef USE_ESC_SENSOR
+    uint32_t escRPM;
+    int8_t escTemperature;
+#endif
 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
 
 //From rc_controls.c
@@ -866,7 +875,7 @@ static void writeIntraframe(void)
     blackboxLoggedAnyFrames = true;
 }
 
-static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHistory, int count)
+static void blackboxWriteArrayUsingAveragePredictor16(int arrOffsetInHistory, int count)
 {
     int16_t *curr  = (int16_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
     int16_t *prev1 = (int16_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
@@ -880,6 +889,20 @@ static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHist
     }
 }
 
+static void blackboxWriteArrayUsingAveragePredictor32(int arrOffsetInHistory, int count)
+{
+    int32_t *curr  = (int32_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
+    int32_t *prev1 = (int32_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
+    int32_t *prev2 = (int32_t*) ((char*) (blackboxHistory[2]) + arrOffsetInHistory);
+
+    for (int i = 0; i < count; i++) {
+        // Predictor is the average of the previous two history states
+        int32_t predictor = ((int64_t)prev1[i] + (int64_t)prev2[i]) / 2;
+
+        blackboxWriteSignedVB(curr[i] - predictor);
+    }
+}
+
 static void writeInterframe(void)
 {
     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
@@ -1014,16 +1037,16 @@ static void writeInterframe(void)
     blackboxWriteTag8_8SVB(deltas, optionalFieldCount);
 
     //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
-    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC),   XYZ_AXIS_COUNT);
-    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
-    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
+    blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroADC),   XYZ_AXIS_COUNT);
+    blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
+    blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
-        blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT);
+        blackboxWriteArrayUsingAveragePredictor32(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT);
     }
-    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor),     getMotorCount());
+    blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, motor),     getMotorCount());
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
-        blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, servo),     MAX_SUPPORTED_SERVOS);
+        blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo),     MAX_SUPPORTED_SERVOS);
     }
 
 #ifdef NAV_BLACKBOX
@@ -1104,6 +1127,10 @@ static void writeSlowFrame(void)
     blackboxWriteSigned16VBArray(slowHistory.tempSensorTemperature, MAX_TEMP_SENSORS);
 #endif
 
+#ifdef USE_ESC_SENSOR
+    blackboxWriteUnsignedVB(slowHistory.escRPM);
+    blackboxWriteSignedVB(slowHistory.escTemperature);
+#endif
     blackboxSlowFrameIterationTimer = 0;
 }
 
@@ -1156,6 +1183,11 @@ static void loadSlowState(blackboxSlowState_t *slow)
     }
 #endif
 
+#ifdef USE_ESC_SENSOR
+    escSensorData_t * escSensor = escSensorGetData(); 
+    slow->escRPM = escSensor->rpm;
+    slow->escTemperature = escSensor->temperature;
+#endif
 }
 
 /**
diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index e0cfb5cdc3..53c165d3c6 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -17,25 +17,9 @@
 
 #pragma once
 
-// only set_BASEPRI is implemented in device library. It does always create memory barrier
-// missing versions are implemented here
-
 #ifdef UNIT_TEST
 static inline void __set_BASEPRI(uint32_t basePri) {(void)basePri;}
 static inline void __set_BASEPRI_MAX(uint32_t basePri) {(void)basePri;}
-static inline void __set_BASEPRI_nb(uint32_t basePri) {(void)basePri;}
-static inline void __set_BASEPRI_MAX_nb(uint32_t basePri) {(void)basePri;}
-#else
-// set BASEPRI and BASEPRI_MAX register, but do not create memory barrier
-__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_nb(uint32_t basePri)
-{
-   __ASM volatile ("\tMSR basepri, %0\n" : : "r" (basePri) );
-}
-
-__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint32_t basePri)
-{
-   __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) );
-}
 #endif // UNIT_TEST
 
 // cleanup BASEPRI restore function, with global memory barrier
@@ -51,68 +35,19 @@ static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
     return 1;
 }
 
-// cleanup BASEPRI restore function, no memory barrier
-static inline void __basepriRestore(uint8_t *val)
-{
-    __set_BASEPRI_nb(*val);
-}
-
-// set BASEPRI_MAX function, no memory barrier, returns true
-static inline uint8_t __basepriSetRetVal(uint8_t prio)
-{
-    __set_BASEPRI_MAX_nb(prio);
-    return 1;
-}
+// The CMSIS provides the function __set_BASEPRI(priority) for changing the value of the BASEPRI register.
+// The function uses the hardware convention for the ‘priority’ argument, which means that the priority must
+// be shifted left by the number of unimplemented bits (8 – __NVIC_PRIO_BITS).
+//
+// NOTE: The priority numbering convention used in __set_BASEPRI(priority) is thus different than in the
+// NVIC_SetPriority(priority) function, which expects the “priority” argument not shifted.
 
 // Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled
 // Full memory barrier is placed at start and exit of block
 #ifdef UNIT_TEST
 #define ATOMIC_BLOCK(prio) {}
-#define ATOMIC_BLOCK_NB(prio) {}
 #else
 #define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
-                                     __ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 )
-
-// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier.
-// Be careful when using this, you must use some method to prevent optimizer form breaking things
-// - lto is used for Cleanflight compilation, so function call is not memory barrier
-// - use ATOMIC_BARRIER or proper volatile to protect used variables
-// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
-//    but that can change in future versions
-#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
-                                    __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
+                                     __ToDo = __basepriSetMemRetVal((prio) << (8U - __NVIC_PRIO_BITS)); __ToDo ; __ToDo = 0 )
 
 #endif // UNIT_TEST
-
-// ATOMIC_BARRIER
-// Create memory barrier
-// - at the beginning (all data must be reread from memory)
-// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use)
-// ideally this would only protect memory passed as parameter (any type should work), but gcc is currently creating almost full barrier
-// this macro can be used only ONCE PER LINE, but multiple uses per block are fine
-
-#if (__GNUC__ > 9)
-#warning "Please verify that ATOMIC_BARRIER works as intended"
-// increment version number is BARRIER works
-// TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead
-// you should check that local variable scope with cleanup spans entire block
-#endif
-
-#ifndef __UNIQL
-# define __UNIQL_CONCAT2(x,y) x ## y
-# define __UNIQL_CONCAT(x,y) __UNIQL_CONCAT2(x,y)
-# define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__)
-#endif
-
-// this macro uses local function for cleanup. CLang block can be substituted
-#define ATOMIC_BARRIER(data)                                            \
-    __extension__ void  __UNIQL(__barrierEnd)(typeof(data) **__d) {     \
-        __asm__ volatile ("\t# barier(" #data ")  end\n" : : "m" (**__d));                          \
-    }                                                                   \
-    typeof(data)  __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \
-    __asm__ volatile ("\t# barier (" #data ") start\n" : "+m" (*__UNIQL(__barrier)))
-
-
-// define these wrappers for atomic operations, use gcc buildins
-#define ATOMIC_OR(ptr, val) __sync_fetch_and_or(ptr, val)
-#define ATOMIC_AND(ptr, val) __sync_fetch_and_and(ptr, val)
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index a300b70ade..283032b297 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -70,5 +70,6 @@ typedef enum {
     DEBUG_NAV_YAW,
     DEBUG_DYNAMIC_FILTER,
     DEBUG_DYNAMIC_FILTER_FREQUENCY,
+    DEBUG_IRLOCK,
     DEBUG_COUNT
 } debugType_e;
diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index fcfb56ebea..ee3742b127 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -766,7 +766,7 @@ void cmsMenuOpen(void)
 {
     if (!cmsInMenu) {
         // New open
-	setServoOutputEnabled(false);
+        setServoOutputEnabled(false);
         pCurrentDisplay = cmsDisplayPortSelectCurrent();
         if (!pCurrentDisplay)
             return;
diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c
index 6fdaae6954..d16a3c79fc 100644
--- a/src/main/cms/cms_menu_blackbox.c
+++ b/src/main/cms/cms_menu_blackbox.c
@@ -36,6 +36,7 @@
 
 #include "config/feature.h"
 
+#include "drivers/flash.h"
 #include "drivers/time.h"
 
 #include "fc/config.h"
@@ -53,7 +54,7 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
     displayResync(pDisplay); // Was max7456RefreshAll(); Why at this timing?
 
     flashfsEraseCompletely();
-    while (!flashfsIsReady()) {
+    while (!flashIsReady()) {
         delay(100);
     }
 
diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c
index 01532efb8f..af5ac8574b 100644
--- a/src/main/cms/cms_menu_navigation.c
+++ b/src/main/cms/cms_menu_navigation.c
@@ -108,6 +108,7 @@ static const OSD_Entry cmsx_menuFixedWingEntries[] =
     OSD_SETTING_ENTRY("MAX DIVE ANGLE", SETTING_NAV_FW_DIVE_ANGLE),
     OSD_SETTING_ENTRY("PITCH TO THR RATIO", SETTING_NAV_FW_PITCH2THR),
     OSD_SETTING_ENTRY("LOITER RADIUS", SETTING_NAV_FW_LOITER_RADIUS),
+    OSD_SETTING_ENTRY("CONTROL SMOOTHNESS", SETTING_NAV_FW_CONTROL_SMOOTHNESS),
 
     OSD_BACK_AND_END_ENTRY,
 };
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index d902acb26b..901bb0e171 100755
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -273,6 +273,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
 
 #ifdef USE_ESC_SENSOR
     OSD_ELEMENT_ENTRY("ESC RPM", OSD_ESC_RPM),
+    OSD_ELEMENT_ENTRY("ESC TEMPERATURE", OSD_ESC_TEMPERATURE),
 #endif
 
     OSD_BACK_AND_END_ENTRY,
@@ -378,6 +379,7 @@ static const OSD_Entry menuOsdHud2Entries[] = {
     OSD_SETTING_ENTRY("RADAR MIN RANGE", SETTING_OSD_HUD_RADAR_RANGE_MIN),
     OSD_SETTING_ENTRY("RADAR MAX RANGE", SETTING_OSD_HUD_RADAR_RANGE_MAX),
     OSD_SETTING_ENTRY("RADAR DET. NEAREST", SETTING_OSD_HUD_RADAR_NEAREST),
+    OSD_SETTING_ENTRY("NEXT WAYPOINTS", SETTING_OSD_HUD_WP_DISP),
     OSD_BACK_ENTRY,
     OSD_END_ENTRY,
 };
diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c
index bf35aa8997..249683fe87 100644
--- a/src/main/cms/cms_menu_vtx.c
+++ b/src/main/cms/cms_menu_vtx.c
@@ -142,7 +142,7 @@ static void cms_Vtx_initSettings(void)
         vtxPitMode = onoff ? 2 : 1;
     }
     else {
-        vtxPitMode = vtxSettingsConfig()->lowPowerDisarm ? 2 : 1;
+        vtxPitMode = 0;
     }
 }
 
@@ -165,7 +165,6 @@ static long cms_Vtx_Commence(displayPort_t *pDisp, const void *self)
     vtxSettingsConfigMutable()->band = vtxBand;
     vtxSettingsConfigMutable()->channel = vtxChan;
     vtxSettingsConfigMutable()->power = vtxPower;
-    vtxSettingsConfigMutable()->lowPowerDisarm = (vtxPitMode == 2 ? 1 : 0);
 
     saveConfigAndNotify();
 
diff --git a/src/main/common/bitarray.c b/src/main/common/bitarray.c
index 92332b9e33..a46a536e3d 100755
--- a/src/main/common/bitarray.c
+++ b/src/main/common/bitarray.c
@@ -75,17 +75,19 @@ int bitArrayFindFirstSet(const bitarrayElement_t *array, unsigned start, size_t
     const uint32_t *end = ptr + (size / 4);
     const uint32_t *p = ptr + start / (8 * 4);
     int ret;
-    // First iteration might need to mask some bits
-    uint32_t mask = 0xFFFFFFFF << (start % (8 * 4));
-    if ((ret = __CTZ(*p & mask)) != 32) {
-        return (((char *)p) - ((char *)ptr)) * 8 + ret;
-    }
-    p++;
-    while (p < end) {
-        if ((ret = __CTZ(*p)) != 32) {
+    if (p < end) {
+        // First iteration might need to mask some bits
+        uint32_t mask = 0xFFFFFFFF << (start % (8 * 4));
+        if ((ret = __CTZ(*p & mask)) != 32) {
             return (((char *)p) - ((char *)ptr)) * 8 + ret;
         }
         p++;
+        while (p < end) {
+            if ((ret = __CTZ(*p)) != 32) {
+                return (((char *)p) - ((char *)ptr)) * 8 + ret;
+            }
+            p++;
+        }
     }
     return -1;
 }
diff --git a/src/main/common/calibration.h b/src/main/common/calibration.h
index a6443aae48..6c84c4437c 100644
--- a/src/main/common/calibration.h
+++ b/src/main/common/calibration.h
@@ -33,7 +33,7 @@
 #include "common/vector.h"
 
 typedef enum {
-    ZERO_CALIBRATION_NONE,
+    ZERO_CALIBRATION_NONE = 0,
     ZERO_CALIBRATION_IN_PROGRESS,
     ZERO_CALIBRATION_DONE,
     ZERO_CALIBRATION_FAIL,
diff --git a/src/main/common/crc.c b/src/main/common/crc.c
index 87c3486eda..d11d37ae9e 100644
--- a/src/main/common/crc.c
+++ b/src/main/common/crc.c
@@ -132,3 +132,14 @@ uint8_t crc8_update(uint8_t crc, const void *data, uint32_t length)
     }
     return crc;
 }
+
+uint8_t crc8_sum_update(uint8_t crc, const void *data, uint32_t length)
+{
+    const uint8_t *p = (const uint8_t *)data;
+    const uint8_t *pend = p + length;
+
+    for (; p != pend; p++) {
+        crc += *p;
+    }
+    return crc;
+}
\ No newline at end of file
diff --git a/src/main/common/crc.h b/src/main/common/crc.h
index 2161838e34..9e02cc88d5 100644
--- a/src/main/common/crc.h
+++ b/src/main/common/crc.h
@@ -34,3 +34,5 @@ void crc8_xor_sbuf_append(struct sbuf_s *dst, uint8_t *start);
 
 uint8_t crc8(uint8_t crc, uint8_t a);
 uint8_t crc8_update(uint8_t crc, const void *data, uint32_t length);
+
+uint8_t crc8_sum_update(uint8_t crc, const void *data, uint32_t length);
\ No newline at end of file
diff --git a/src/main/common/encoding.c b/src/main/common/encoding.c
index 136f628031..32d9eb5205 100644
--- a/src/main/common/encoding.c
+++ b/src/main/common/encoding.c
@@ -15,6 +15,10 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "platform.h"
+
+FILE_COMPILE_FOR_SPEED
+
 #include "encoding.h"
 
 /**
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 3226e52b3a..c2eb1cf4cc 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
@@ -116,10 +118,9 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
     return filter->state;
 }
 
-float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff)
+float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz)
 {
-    const float octaves = log2f((float)centerFreq  / (float)cutoff) * 2;
-    return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
+    return centerFrequencyHz * cutoffFrequencyHz / (centerFrequencyHz * centerFrequencyHz - cutoffFrequencyHz * cutoffFrequencyHz);
 }
 
 void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz)
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 5ce181dba6..fadae7ba48 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -79,7 +79,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
-float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
+float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
diff --git a/src/main/common/global_functions.c b/src/main/common/global_functions.c
index 7421dab33c..5e12bf9761 100644
--- a/src/main/common/global_functions.c
+++ b/src/main/common/global_functions.c
@@ -98,6 +98,22 @@ void globalFunctionsProcess(int8_t functionId) {
                     }
                 }
                 break;
+            case GLOBAL_FUNCTION_ACTION_SET_VTX_BAND:
+                if (conditionValue && !previousValue) {
+                    vtxDeviceCapability_t vtxDeviceCapability;
+                    if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+                        vtxSettingsConfigMutable()->band = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
+                    }
+                }
+                break;
+            case GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL:
+                if (conditionValue && !previousValue) {
+                    vtxDeviceCapability_t vtxDeviceCapability;
+                    if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+                        vtxSettingsConfigMutable()->channel = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
+                    }
+                }
+                break;
             case GLOBAL_FUNCTION_ACTION_INVERT_ROLL:
                 if (conditionValue) {
                     GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_ROLL);
@@ -142,7 +158,7 @@ float NOINLINE getThrottleScale(float globalThrottleScale) {
     }
 }
 
-int16_t FAST_CODE getRcCommandOverride(int16_t command[], uint8_t axis) {
+int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
     int16_t outputValue = command[axis];
 
     if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {
diff --git a/src/main/common/global_functions.h b/src/main/common/global_functions.h
index d31157a310..f8e0128eab 100644
--- a/src/main/common/global_functions.h
+++ b/src/main/common/global_functions.h
@@ -29,14 +29,16 @@
 #define MAX_GLOBAL_FUNCTIONS 8
 
 typedef enum {
-    GLOBAL_FUNCTION_ACTION_OVERRIDE_ARMING_SAFETY = 0,
-    GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE,
-    GLOBAL_FUNCTION_ACTION_SWAP_ROLL_YAW,
-    GLOBAL_FUNCTION_ACTION_SET_VTX_POWER_LEVEL,
-    GLOBAL_FUNCTION_ACTION_INVERT_ROLL,
-    GLOBAL_FUNCTION_ACTION_INVERT_PITCH,
-    GLOBAL_FUNCTION_ACTION_INVERT_YAW,
-    GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE,
+    GLOBAL_FUNCTION_ACTION_OVERRIDE_ARMING_SAFETY = 0,      // 0
+    GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE,         // 1
+    GLOBAL_FUNCTION_ACTION_SWAP_ROLL_YAW,                   // 2
+    GLOBAL_FUNCTION_ACTION_SET_VTX_POWER_LEVEL,             // 3
+    GLOBAL_FUNCTION_ACTION_INVERT_ROLL,                     // 4
+    GLOBAL_FUNCTION_ACTION_INVERT_PITCH,                    // 5
+    GLOBAL_FUNCTION_ACTION_INVERT_YAW,                      // 6
+    GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE,               // 7
+    GLOBAL_FUNCTION_ACTION_SET_VTX_BAND,                    // 8
+    GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL,                 // 9
     GLOBAL_FUNCTION_ACTION_LAST
 } globalFunctionActions_e;
 
diff --git a/src/main/common/global_variables.c b/src/main/common/global_variables.c
new file mode 100644
index 0000000000..72230a97ae
--- /dev/null
+++ b/src/main/common/global_variables.c
@@ -0,0 +1,72 @@
+/*
+ * 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_LOGIC_CONDITIONS
+
+#include <stdint.h>
+#include "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+#include "common/global_variables.h"
+#include "common/maths.h"
+#include "build/build_config.h"
+ 
+static EXTENDED_FASTRAM int32_t globalVariableState[MAX_GLOBAL_VARIABLES];
+
+PG_REGISTER_ARRAY_WITH_RESET_FN(globalVariableConfig_t, MAX_GLOBAL_VARIABLES, globalVariableConfigs, PG_GLOBAL_VARIABLE_CONFIG, 0);
+
+void pgResetFn_globalVariableConfigs(globalVariableConfig_t *globalVariableConfigs)
+{
+    for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
+        globalVariableConfigs[i].min = INT16_MIN;
+        globalVariableConfigs[i].max = INT16_MAX;
+        globalVariableConfigs[i].defaultValue = 0;
+    }
+}
+
+int32_t gvGet(uint8_t index) {
+    if (index < MAX_GLOBAL_VARIABLES) {
+        return globalVariableState[index];
+    } else {
+        return 0;
+    }
+}
+
+void gvSet(uint8_t index, int32_t value) {
+    if (index < MAX_GLOBAL_VARIABLES) {
+        globalVariableState[index] = constrain(value, globalVariableConfigs(index)->min, globalVariableConfigs(index)->max);
+    }
+}
+
+void gvInit(void) {
+    for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
+        globalVariableState[i] = globalVariableConfigs(i)->defaultValue;
+    }
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/common/global_variables.h b/src/main/common/global_variables.h
new file mode 100644
index 0000000000..cbeb500348
--- /dev/null
+++ b/src/main/common/global_variables.h
@@ -0,0 +1,40 @@
+/*
+ * 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"
+
+#define MAX_GLOBAL_VARIABLES 4
+
+typedef struct globalVariableConfig_s {
+    int32_t min;
+    int32_t max;
+    int32_t defaultValue;
+} globalVariableConfig_t;
+PG_DECLARE_ARRAY(globalVariableConfig_t, MAX_GLOBAL_VARIABLES, globalVariableConfigs);
+
+int32_t gvGet(uint8_t index);
+void gvSet(uint8_t index, int32_t value);
+void gvInit(void);
\ No newline at end of file
diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c
index 38f71b134c..7ba9dd2f1b 100644
--- a/src/main/common/logic_condition.c
+++ b/src/main/common/logic_condition.c
@@ -29,6 +29,7 @@
 #include "config/parameter_group_ids.h"
 
 #include "common/logic_condition.h"
+#include "common/global_variables.h"
 #include "common/utils.h"
 #include "rx/rx.h"
 #include "maths.h"
@@ -39,11 +40,32 @@
 #include "sensors/battery.h"
 #include "sensors/pitotmeter.h"
 #include "flight/imu.h"
+#include "flight/pid.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
-PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0);
+PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 1);
+
+void pgResetFn_logicConditions(logicCondition_t *instance)
+{
+    for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
+        RESET_CONFIG(logicCondition_t, &instance[i],
+            .enabled = 0,
+            .activatorId = -1,
+            .operation = 0,
+            .operandA = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .operandB = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .flags = 0
+        );
+    }
+}
 
 logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
 
@@ -53,6 +75,7 @@ static int logicConditionCompute(
     int operandA,
     int operandB
 ) {
+    int temporaryValue;
     switch (operation) {
 
         case LOGIC_CONDITION_TRUE:
@@ -121,6 +144,43 @@ static int logicConditionCompute(
             return currentVaue;
             break;
 
+        case LOGIC_CONDITION_GVAR_SET:
+            gvSet(operandA, operandB);
+            return operandB;
+            break;
+        
+        case LOGIC_CONDITION_GVAR_INC:
+            temporaryValue = gvGet(operandA) + operandB;
+            gvSet(operandA, temporaryValue);
+            return temporaryValue;
+            break;
+
+        case LOGIC_CONDITION_GVAR_DEC:
+            temporaryValue = gvGet(operandA) - operandB;
+            gvSet(operandA, temporaryValue);
+            return temporaryValue;
+            break;
+
+        case LOGIC_CONDITION_ADD:
+            return constrain(operandA + operandB, INT16_MIN, INT16_MAX);
+            break;
+
+        case LOGIC_CONDITION_SUB:
+            return constrain(operandA - operandB, INT16_MIN, INT16_MAX);
+            break;
+
+        case LOGIC_CONDITION_MUL:
+            return constrain(operandA * operandB, INT16_MIN, INT16_MAX);
+            break;
+
+        case LOGIC_CONDITION_DIV:
+            if (operandB != 0) {
+                return constrain(operandA / operandB, INT16_MIN, INT16_MAX);
+            } else {
+                return operandA;
+            }
+            break;
+
         default:
             return false;
             break; 
@@ -129,7 +189,9 @@ static int logicConditionCompute(
 
 void logicConditionProcess(uint8_t i) {
 
-    if (logicConditions(i)->enabled) {
+    const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
+
+    if (logicConditions(i)->enabled && activatorValue) {
         
         /*
          * Process condition only when latch flag is not set
@@ -164,15 +226,15 @@ static int logicConditionGetFlightOperandValue(int operand) {
     switch (operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
-            return constrain((uint32_t)getFlightTime(), 0, 32767);
+            return constrain((uint32_t)getFlightTime(), 0, INT16_MAX);
             break;
         
         case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
-            return constrain(GPS_distanceToHome, 0, 32767);
+            return constrain(GPS_distanceToHome, 0, INT16_MAX);
             break;
         
         case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
-            return constrain(getTotalTravelDistance() / 100, 0, 32767);
+            return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
@@ -209,18 +271,18 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s
         #ifdef USE_PITOT
-            return constrain(pitot.airSpeed, 0, 32767);
+            return constrain(pitot.airSpeed, 0, INT16_MAX);
         #else
             return false;
         #endif
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm
-            return constrain(getEstimatedActualPosition(Z), -32678, 32767);
+            return constrain(getEstimatedActualPosition(Z), INT16_MIN, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
-            return constrain(getEstimatedActualVelocity(Z), 0, 32767);
+            return constrain(getEstimatedActualVelocity(Z), 0, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %
@@ -269,7 +331,19 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
             return (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) ? 1 : 0;
-            break; 
+            break;
+        
+        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // 
+            return axisPID[YAW];
+            break;
+        
+        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // 
+            return axisPID[ROLL];
+            break;
+        
+        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // 
+            return axisPID[PITCH];
+            break;
 
         default:
             return 0;
@@ -353,6 +427,12 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
             }
             break;
 
+        case LOGIC_CONDITION_OPERAND_TYPE_GVAR:
+            if (operand >= 0 && operand < MAX_GLOBAL_VARIABLES) {
+                retVal = gvGet(operand);
+            }
+            break;
+
         default:
             break;
     }
diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h
index 10129db72c..f48383870a 100644
--- a/src/main/common/logic_condition.h
+++ b/src/main/common/logic_condition.h
@@ -43,6 +43,13 @@ typedef enum {
     LOGIC_CONDITION_NOR,            // 11
     LOGIC_CONDITION_NOT,            // 12
     LOGIC_CONDITION_STICKY,         // 13
+    LOGIC_CONDITION_ADD,            // 14
+    LOGIC_CONDITION_SUB,            // 15
+    LOGIC_CONDITION_MUL,            // 16
+    LOGIC_CONDITION_DIV,            // 17
+    LOGIC_CONDITION_GVAR_SET,       // 18
+    LOGIC_CONDITION_GVAR_INC,       // 19
+    LOGIC_CONDITION_GVAR_DEC,       // 20
     LOGIC_CONDITION_LAST
 } logicOperation_e;
 
@@ -52,6 +59,7 @@ typedef enum logicOperandType_s {
     LOGIC_CONDITION_OPERAND_TYPE_FLIGHT,
     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_LAST
 } logicOperandType_e;
 
@@ -82,6 +90,9 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP, // 0/1                            // 23
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1                       // 24
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1                      // 25
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL,                         // 26
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH,                        // 27
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW,                          // 28
 } logicFlightOperands_e;
 
 typedef enum {
@@ -107,6 +118,7 @@ typedef struct logicOperand_s {
 
 typedef struct logicCondition_s {
     uint8_t enabled;
+    int8_t activatorId;
     logicOperation_e operation;
     logicOperand_t operandA;
     logicOperand_t operandB;
@@ -126,4 +138,4 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand);
 
 int logicConditionGetValue(int8_t conditionId);
 void logicConditionUpdateTask(timeUs_t currentTimeUs);
-void logicConditionReset(void);
\ No newline at end of file
+void logicConditionReset(void);
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 13d615e361..cb7904252e 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -25,6 +25,8 @@
 #include "quaternion.h"
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 // http://lolengine.net/blog/2011/12/21/better-function-approximations
 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
 // Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
@@ -159,7 +161,7 @@ int constrain(int amt, int low, int high)
         return amt;
 }
 
-float FAST_CODE NOINLINE constrainf(float amt, float low, float high)
+float constrainf(float amt, float low, float high)
 {
     if (amt < low)
         return low;
@@ -473,7 +475,19 @@ static void sensorCalibration_SolveLGS(float A[4][4], float x[4], float b[4]) {
     sensorCalibration_BackwardSubstitution(A, x, y);
 }
 
-void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
+bool sensorCalibrationValidateResult(const float result[3])
+{
+    // Validate that result is not INF and not NAN
+    for (int i = 0; i < 3; i++) {
+        if (isnan(result[i]) && isinf(result[i])) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
 {
     float beta[4];
     sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
@@ -481,9 +495,11 @@ void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float res
     for (int i = 0; i < 3; i++) {
         result[i] = beta[i] / 2;
     }
+
+    return sensorCalibrationValidateResult(result);
 }
 
-void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
+bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
 {
     float beta[4];
     sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
@@ -491,6 +507,8 @@ void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
     for (int i = 0; i < 3; i++) {
         result[i] = sqrtf(beta[i]);
     }
+
+    return sensorCalibrationValidateResult(result);
 }
 
 float bellCurve(const float x, const float curveWidth)
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index 4a4cee9fe8..728df5e57a 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -18,6 +18,7 @@
 #pragma once
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #ifndef sq
 #define sq(x) ((x)*(x))
@@ -59,6 +60,8 @@
 #define CENTIMETERS_TO_FEET(cm)                 (cm * (328 / 10000.0))
 #define CENTIMETERS_TO_METERS(cm)               (cm / 100)
 
+#define METERS_TO_CENTIMETERS(m)                (m * 100)
+
 // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar)         \
     ( __extension__ ({                                      \
@@ -122,8 +125,8 @@ typedef struct {
 void sensorCalibrationResetState(sensorCalibrationState_t * state);
 void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]);
 void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target);
-void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
-void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
+bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
+bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
 
 int gcd(int num, int denom);
 int32_t applyDeadband(int32_t value, int32_t deadband);
diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c
index 031aed6c77..b07f41f8a0 100644
--- a/src/main/common/typeconversion.c
+++ b/src/main/common/typeconversion.c
@@ -20,6 +20,8 @@
 #include "build/build_config.h"
 
 #include "maths.h"
+#include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
 
@@ -185,7 +187,7 @@ char *ftoa(float x, char *floatString)
 
     dpLocation = strlen(intString2) - 3;
 
-    strncpy(floatString, intString2, dpLocation);
+    memcpy(floatString, intString2, dpLocation);
     floatString[dpLocation] = '\0';
     strcat(floatString, decimalPoint);
     strcat(floatString, intString2 + dpLocation);
diff --git a/src/main/config/feature.c b/src/main/config/feature.c
index 67715427e0..042509629d 100755
--- a/src/main/config/feature.c
+++ b/src/main/config/feature.c
@@ -31,12 +31,13 @@ void latchActiveFeatures(void)
 
 bool featureConfigured(uint32_t mask)
 {
-    return featureConfig()->enabledFeatures & mask;
+    return (featureConfig()->enabledFeatures & mask) == mask;
 }
 
-bool FAST_CODE NOINLINE feature(uint32_t mask)
+bool feature(uint32_t mask)
 {
-    return activeFeaturesLatch & mask;
+    // Check for ALL masked features
+    return (activeFeaturesLatch & mask) == mask;
 }
 
 void featureSet(uint32_t mask)
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 09ad096d3e..104a3e4627 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -110,7 +110,8 @@
 #define PG_GLOBAL_FUNCTIONS 1020
 #define PG_ESC_SENSOR_CONFIG 1021
 #define PG_RPM_FILTER_CONFIG 1022
-#define PG_INAV_END 1022
+#define PG_GLOBAL_VARIABLE_CONFIG 1023
+#define PG_INAV_END 1023
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
diff --git a/src/main/drivers/accgyro/accgyro_adxl345.c b/src/main/drivers/accgyro/accgyro_adxl345.c
index e2e5dc3c2c..b867f85b4a 100644
--- a/src/main/drivers/accgyro/accgyro_adxl345.c
+++ b/src/main/drivers/accgyro/accgyro_adxl345.c
@@ -28,7 +28,7 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_adxl345.h"
 
-#ifdef USE_ACC_ADXL345
+#ifdef USE_IMU_ADXL345
 
 // ADXL345, Alternative address mode 0x53
 #define ADXL345_ADDRESS     0x53
@@ -110,6 +110,7 @@ bool adxl345Detect(accDev_t *acc)
 
     acc->initFn = adxl345Init;
     acc->readFn = adxl345Read;
+    acc->accAlign = acc->busDev->param;
     return true;
 }
 
diff --git a/src/main/drivers/accgyro/accgyro_bma280.c b/src/main/drivers/accgyro/accgyro_bma280.c
index 0f57186bcb..a97b831bd2 100644
--- a/src/main/drivers/accgyro/accgyro_bma280.c
+++ b/src/main/drivers/accgyro/accgyro_bma280.c
@@ -27,7 +27,7 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_bma280.h"
 
-#ifdef USE_ACC_BMA280
+#ifdef USE_IMU_BMA280
 
 // BMA280, default I2C address mode 0x18
 #define BMA280_WHOAMI       0x00
@@ -86,6 +86,7 @@ bool bma280Detect(accDev_t *acc)
 
     acc->initFn = bma280Init;
     acc->readFn = bma280Read;
+    acc->accAlign = acc->busDev->param;
     return true;
 }
 
diff --git a/src/main/drivers/accgyro/accgyro_bmi160.c b/src/main/drivers/accgyro/accgyro_bmi160.c
index e84a9046fc..60b54f7f8c 100644
--- a/src/main/drivers/accgyro/accgyro_bmi160.c
+++ b/src/main/drivers/accgyro/accgyro_bmi160.c
@@ -42,7 +42,7 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_bmi160.h"
 
-#if defined(USE_GYRO_BMI160) || defined(USE_ACC_BMI160)
+#if defined(USE_IMU_BMI160)
 
 /* BMI160 Registers */
 #define BMI160_REG_CHIPID           0x00
@@ -270,6 +270,7 @@ bool bmi160GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = NULL;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_fake.c b/src/main/drivers/accgyro/accgyro_fake.c
index 473691c20e..0cd9addfae 100644
--- a/src/main/drivers/accgyro/accgyro_fake.c
+++ b/src/main/drivers/accgyro/accgyro_fake.c
@@ -27,7 +27,7 @@
 #include "drivers/accgyro/accgyro_fake.h"
 
 
-#ifdef USE_FAKE_GYRO
+#ifdef USE_IMU_FAKE
 
 static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
 
@@ -71,6 +71,7 @@ bool fakeGyroDetect(gyroDev_t *gyro)
     gyro->readFn = fakeGyroRead;
     gyro->temperatureFn = fakeGyroReadTemperature;
     gyro->scale = 1.0f / 16.4f;
+    gyro->gyroAlign = 0;
     return true;
 }
 #endif // USE_FAKE_GYRO
@@ -104,6 +105,7 @@ bool fakeAccDetect(accDev_t *acc)
 {
     acc->initFn = fakeAccInit;
     acc->readFn = fakeAccRead;
+    acc->accAlign = 0;
     return true;
 }
 #endif // USE_FAKE_ACC
diff --git a/src/main/drivers/accgyro/accgyro_icm20689.c b/src/main/drivers/accgyro/accgyro_icm20689.c
index 4766e5aa94..6ed891dac3 100644
--- a/src/main/drivers/accgyro/accgyro_icm20689.c
+++ b/src/main/drivers/accgyro/accgyro_icm20689.c
@@ -38,7 +38,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_icm20689.h"
 
-#if (defined(USE_GYRO_ICM20689) || defined(USE_ACC_ICM20689))
+#if defined(USE_IMU_ICM20689)
 
 static uint8_t icm20689DeviceDetect(const busDevice_t *busDev)
 {
@@ -82,6 +82,7 @@ bool icm20689AccDetect(accDev_t *acc)
 
     acc->initFn = icm20689AccInit;
     acc->readFn = mpuAccReadScratchpad;
+    acc->accAlign = acc->busDev->param;
 
     return true;
 }
@@ -142,6 +143,7 @@ bool icm20689GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_icm20689.h b/src/main/drivers/accgyro/accgyro_icm20689.h
index bf37b1d4ad..44911daebd 100644
--- a/src/main/drivers/accgyro/accgyro_icm20689.h
+++ b/src/main/drivers/accgyro/accgyro_icm20689.h
@@ -24,9 +24,5 @@
 
 #define ICM20689_BIT_RESET                  (0x80)
 
-#if (defined(USE_GYRO_ICM20689) || defined(USE_ACC_ICM20689))
-
 bool icm20689AccDetect(accDev_t *acc);
 bool icm20689GyroDetect(gyroDev_t *gyro);
-
-#endif
diff --git a/src/main/drivers/accgyro/accgyro_l3g4200d.c b/src/main/drivers/accgyro/accgyro_l3g4200d.c
index 1fee8f712f..e674973bf2 100644
--- a/src/main/drivers/accgyro/accgyro_l3g4200d.c
+++ b/src/main/drivers/accgyro/accgyro_l3g4200d.c
@@ -139,6 +139,7 @@ bool l3g4200dDetect(gyroDev_t *gyro)
     gyro->initFn = l3g4200dInit;
     gyro->readFn = l3g4200dRead;
     gyro->scale = 1.0f / 14.2857f;      // 14.2857dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_l3gd20.c b/src/main/drivers/accgyro/accgyro_l3gd20.c
index 29e78fe231..9fc09556cf 100644
--- a/src/main/drivers/accgyro/accgyro_l3gd20.c
+++ b/src/main/drivers/accgyro/accgyro_l3gd20.c
@@ -32,7 +32,7 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_l3gd20.h"
 
-#ifdef USE_GYRO_L3GD20
+#ifdef USE_IMU_L3GD20
 
 #define READ_CMD               ((uint8_t)0x80)
 #define MULTIPLEBYTE_CMD       ((uint8_t)0x40)
@@ -116,6 +116,7 @@ bool l3gd20Detect(gyroDev_t *gyro)
 
     // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
     gyro->scale = 0.07f;
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro/accgyro_lsm303dlhc.c
index 6f7f84b830..5421d8e337 100644
--- a/src/main/drivers/accgyro/accgyro_lsm303dlhc.c
+++ b/src/main/drivers/accgyro/accgyro_lsm303dlhc.c
@@ -20,7 +20,7 @@
 
 #include "platform.h"
 
-#ifdef USE_ACC_LSM303DLHC
+#ifdef USE_IMU_LSM303DLHC
 
 #include "build/debug.h"
 
@@ -163,6 +163,7 @@ bool lsm303dlhcAccDetect(accDev_t *acc)
 
     acc->initFn = lsm303dlhcAccInit;
     acc->readFn = lsm303dlhcAccRead;
+    acc->accAlign = acc->busDev->param;
     return true;
 }
 
diff --git a/src/main/drivers/accgyro/accgyro_mma845x.c b/src/main/drivers/accgyro/accgyro_mma845x.c
index 1f41910f56..0a5a9844aa 100644
--- a/src/main/drivers/accgyro/accgyro_mma845x.c
+++ b/src/main/drivers/accgyro/accgyro_mma845x.c
@@ -139,5 +139,6 @@ bool mma8452Detect(accDev_t *acc)
 
     acc->initFn = mma8452Init;
     acc->readFn = mma8452Read;
+    acc->accAlign = acc->busDev->param;
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.c b/src/main/drivers/accgyro/accgyro_mpu3050.c
index bb3d919fd7..082aa795f5 100644
--- a/src/main/drivers/accgyro/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro/accgyro_mpu3050.c
@@ -33,7 +33,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu3050.h"
 
-#ifdef USE_GYRO_MPU3050
+#ifdef USE_IMU_MPU3050
 
 // MPU3050, Standard address 0x68
 #define MPU3050_ADDRESS         0x68
@@ -131,6 +131,7 @@ bool mpu3050Detect(gyroDev_t *gyro)
     gyro->readFn = mpu3050GyroRead;
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu6000.c b/src/main/drivers/accgyro/accgyro_mpu6000.c
index e0396f29ec..6f8555c0b3 100644
--- a/src/main/drivers/accgyro/accgyro_mpu6000.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6000.c
@@ -42,7 +42,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu6000.h"
 
-#if (defined(USE_GYRO_MPU6000) || defined(USE_ACC_MPU6000))
+#if defined(USE_IMU_MPU6000)
 
 // Bits
 #define BIT_H_RESET                 0x80
@@ -150,6 +150,7 @@ bool mpu6000AccDetect(accDev_t *acc)
 
     acc->initFn = mpu6000AccInit;
     acc->readFn = mpuAccReadScratchpad;
+    acc->accAlign = acc->busDev->param;
 
     return true;
 }
@@ -218,6 +219,7 @@ bool mpu6000GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c
index b6c8671691..5c83f977d7 100755
--- a/src/main/drivers/accgyro/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6050.c
@@ -42,7 +42,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu6050.h"
 
-#if defined(USE_GYRO_MPU6050) || defined(USE_ACC_MPU6050)
+#if defined(USE_IMU_MPU6050)
 
 #define BIT_H_RESET                 0x80
 #define MPU_CLK_SEL_PLLGYROZ        0x03
@@ -127,6 +127,7 @@ bool mpu6050AccDetect(accDev_t *acc)
     if (ctx->chipMagicNumber == 0x6850 || ctx->chipMagicNumber == 0x6050) {
         acc->initFn = mpu6050AccInit;
         acc->readFn = mpuAccReadScratchpad;
+        acc->accAlign = acc->busDev->param;
         return true;
     }
 
@@ -213,6 +214,7 @@ bool mpu6050GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c
index 532fd03606..5d381acbab 100755
--- a/src/main/drivers/accgyro/accgyro_mpu6500.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6500.c
@@ -33,7 +33,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu6500.h"
 
-#if defined(USE_GYRO_MPU6500) || defined(USE_ACC_MPU6500)
+#if defined(USE_IMU_MPU6500)
 
 #define MPU6500_BIT_RESET                   (0x80)
 #define MPU6500_BIT_INT_ANYRD_2CLEAR        (1 << 4)
@@ -60,6 +60,7 @@ bool mpu6500AccDetect(accDev_t *acc)
 
     acc->initFn = mpu6500AccInit;
     acc->readFn = mpuAccReadScratchpad;
+    acc->accAlign = acc->busDev->param;
 
     return true;
 }
@@ -163,6 +164,7 @@ bool mpu6500GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu9250.c b/src/main/drivers/accgyro/accgyro_mpu9250.c
index 03801eeb89..32d7d5e7a2 100755
--- a/src/main/drivers/accgyro/accgyro_mpu9250.c
+++ b/src/main/drivers/accgyro/accgyro_mpu9250.c
@@ -33,7 +33,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu9250.h"
 
-#if defined(USE_GYRO_MPU9250) || defined(USE_ACC_MPU9250)
+#if defined(USE_IMU_MPU9250)
 
 #define MPU9250_BIT_RESET                   (0x80)
 #define MPU9250_BIT_INT_ANYRD_2CLEAR        (1 << 4)
@@ -60,6 +60,7 @@ bool mpu9250AccDetect(accDev_t *acc)
 
     acc->initFn = mpu9250AccInit;
     acc->readFn = mpuAccReadScratchpad;
+    acc->accAlign = acc->busDev->param;
 
     return true;
 }
@@ -163,6 +164,7 @@ bool mpu9250GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c
index 4e93d458f8..038c5029ce 100644
--- a/src/main/drivers/barometer/barometer_bmp280.c
+++ b/src/main/drivers/barometer/barometer_bmp280.c
@@ -166,7 +166,7 @@ static bool deviceDetect(busDevice_t * busDev)
 
         bool ack = busRead(busDev, BMP280_CHIP_ID_REG, &chipId);
 
-        if (ack && chipId == BMP280_DEFAULT_CHIP_ID) {
+        if ((ack && chipId == BMP280_DEFAULT_CHIP_ID)  || (ack && chipId == BME280_DEFAULT_CHIP_ID)){
             return true;
         }
     };
diff --git a/src/main/drivers/barometer/barometer_bmp280.h b/src/main/drivers/barometer/barometer_bmp280.h
index 0060e820b7..d9827fbf0b 100644
--- a/src/main/drivers/barometer/barometer_bmp280.h
+++ b/src/main/drivers/barometer/barometer_bmp280.h
@@ -19,6 +19,7 @@
 
 #define BMP280_I2C_ADDR                      (0x76)
 #define BMP280_DEFAULT_CHIP_ID               (0x58)
+#define BME280_DEFAULT_CHIP_ID               (0x60)
 
 #define BMP280_CHIP_ID_REG                   (0xD0)  /* Chip ID Register */
 #define BMP280_RST_REG                       (0xE0)  /* Softreset Register */
diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c
index e9b8b8f433..62126fe025 100755
--- a/src/main/drivers/bus.c
+++ b/src/main/drivers/bus.c
@@ -32,7 +32,7 @@
 #include "drivers/bus.h"
 #include "drivers/io.h"
 
-#define BUSDEV_MAX_DEVICES 8
+#define BUSDEV_MAX_DEVICES 16
 
 #ifdef USE_SPI
 static void busDevPreInit_SPI(const busDeviceDescriptor_t * descriptor)
@@ -124,6 +124,7 @@ busDevice_t * busDeviceInit(busType_e bus, devHardwareType_e hw, uint8_t tag, re
                 dev->descriptorPtr = descriptor;
                 dev->busType = descriptor->busType;
                 dev->flags = descriptor->flags;
+                dev->param = descriptor->param;
 
                 switch (descriptor->busType) {
                     default:
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 843404573f..7f7d06d092 100755
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -143,11 +143,14 @@ typedef enum {
     DEVHW_M25P16,       // SPI NOR flash
     DEVHW_UG2864,       // I2C OLED display
     DEVHW_SDCARD,       // Generic SD-Card
+    DEVHW_IRLOCK,       // IR-Lock visual positioning hardware
 } devHardwareType_e;
 
 typedef enum {
     DEVFLAGS_NONE                       = 0,
     DEVFLAGS_USE_RAW_REGISTERS          = (1 << 0),     // Don't manipulate MSB for R/W selection (SPI), allow using 0xFF register to raw i2c reads/writes
+
+    // SPI-only
     DEVFLAGS_USE_MANUAL_DEVICE_SELECT   = (1 << 1),     // (SPI only) Don't automatically select/deselect device
     DEVFLAGS_SPI_MODE_0                 = (1 << 2),     // (SPI only) Use CPOL=0/CPHA=0 (if unset MODE3 is used - CPOL=1/CPHA=1)
 } deviceFlags_e;
@@ -156,8 +159,9 @@ typedef struct busDeviceDescriptor_s {
     void *              devicePtr;
     busType_e           busType;
     devHardwareType_e   devHwType;
-    uint16_t            flags;
+    uint8_t             flags;
     uint8_t             tag;
+    uint8_t             param;      // Driver-specific parameter
     union {
 #ifdef USE_SPI
         struct {
@@ -179,6 +183,7 @@ typedef struct busDevice_s {
     const busDeviceDescriptor_t * descriptorPtr;
     busType_e busType;              // Copy of busType to avoid additional pointer dereferencing
     uint32_t flags;                 // Copy of flags
+    uint32_t param;                 // Copy of param
     union {
 #ifdef USE_SPI
         struct {
@@ -209,53 +214,55 @@ extern const busDeviceDescriptor_t __busdev_registry_end[];
 #define BUSDEV_REGISTER_ATTRIBUTES __attribute__ ((section(".busdev_registry"), used, aligned(4)))
 #endif
 
-#define BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags)       \
-    extern const busDeviceDescriptor_t _name ## _registry;                                  \
-    static busDevice_t _name ## _memory;                                                    \
-    const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = {           \
-        .devicePtr = (void *) & _name ## _memory,                                           \
-        .busType = BUSTYPE_SPI,                                                             \
-        .devHwType = _devHw,                                                                \
-        .flags = _flags,                                                                    \
-        .tag = _tag,                                                                        \
-        .busdev.spi = {                                                                     \
-            .spiBus = _spiBus,                                                              \
-            .csnPin = IO_TAG(_csnPin)                                                       \
-        },                                                                                  \
-        .irqPin = IO_TAG(_irqPin)                                                           \
-    };                                                                                      \
-    struct _dummy                                                                           \
+#define BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags, _param)   \
+    extern const busDeviceDescriptor_t _name ## _registry;                                      \
+    static busDevice_t _name ## _memory;                                                        \
+    const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = {               \
+        .devicePtr = (void *) & _name ## _memory,                                               \
+        .busType = BUSTYPE_SPI,                                                                 \
+        .devHwType = _devHw,                                                                    \
+        .flags = _flags,                                                                        \
+        .tag = _tag,                                                                            \
+        .param = _param,                                                                        \
+        .busdev.spi = {                                                                         \
+            .spiBus = _spiBus,                                                                  \
+            .csnPin = IO_TAG(_csnPin)                                                           \
+        },                                                                                      \
+        .irqPin = IO_TAG(_irqPin)                                                               \
+    };                                                                                          \
+    struct _dummy                                                                               \
     /**/
 
-#define BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags)      \
-    extern const busDeviceDescriptor_t _name ## _registry;                                  \
-    static busDevice_t _name ## _memory;                                                    \
-    const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = {           \
-        .devicePtr = (void *) & _name ## _memory,                                           \
-        .busType = BUSTYPE_I2C,                                                             \
-        .devHwType = _devHw,                                                                \
-        .flags = _flags,                                                                    \
-        .tag = _tag,                                                                        \
-        .busdev.i2c = {                                                                     \
-            .i2cBus = _i2cBus,                                                              \
-            .address = _devAddr                                                             \
-        },                                                                                  \
-        .irqPin = IO_TAG(_irqPin)                                                           \
-    };                                                                                      \
-    struct _dummy                                                                           \
+#define BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags, _param)  \
+    extern const busDeviceDescriptor_t _name ## _registry;                                      \
+    static busDevice_t _name ## _memory;                                                        \
+    const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = {               \
+        .devicePtr = (void *) & _name ## _memory,                                               \
+        .busType = BUSTYPE_I2C,                                                                 \
+        .devHwType = _devHw,                                                                    \
+        .flags = _flags,                                                                        \
+        .tag = _tag,                                                                            \
+        .param = _param,                                                                        \
+        .busdev.i2c = {                                                                         \
+            .i2cBus = _i2cBus,                                                                  \
+            .address = _devAddr                                                                 \
+        },                                                                                      \
+        .irqPin = IO_TAG(_irqPin)                                                               \
+    };                                                                                          \
+    struct _dummy                                                                               \
     /**/
 
-#define BUSDEV_REGISTER_SPI(_name, _devHw, _spiBus, _csnPin, _irqPin, _flags)               \
-    BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, 0, _flags)
+#define BUSDEV_REGISTER_SPI(_name, _devHw, _spiBus, _csnPin, _irqPin, _flags, _param)           \
+    BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, 0, _flags, _param)
 
-#define BUSDEV_REGISTER_SPI_TAG(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags)     \
-    BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags)
+#define BUSDEV_REGISTER_SPI_TAG(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags, _param) \
+    BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags, _param)
 
-#define BUSDEV_REGISTER_I2C(_name, _devHw, _i2cBus, _devAddr, _irqPin, _flags)              \
-    BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, 0, _flags)
+#define BUSDEV_REGISTER_I2C(_name, _devHw, _i2cBus, _devAddr, _irqPin, _flags, _param)          \
+    BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, 0, _flags, _param)
 
-#define BUSDEV_REGISTER_I2C_TAG(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags)    \
-    BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags)
+#define BUSDEV_REGISTER_I2C_TAG(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags, _param)\
+    BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags, _param)
 
 
 // busTransfer and busTransferMultiple are supported only on full-duplex SPI bus
diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c
index 1f8c6f2838..e2dc6436c6 100644
--- a/src/main/drivers/bus_i2c_hal.c
+++ b/src/main/drivers/bus_i2c_hal.c
@@ -317,9 +317,10 @@ void i2cInit(I2CDevice device)
     /* Enable the Analog I2C Filter */
     HAL_I2CEx_ConfigAnalogFilter(&i2cHandle[device].Handle,I2C_ANALOGFILTER_ENABLE);
 
-    HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
+    HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIO_I2C_ER, 0);
     HAL_NVIC_EnableIRQ(i2cHardwareMap[device].er_irq);
-    HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
+
+    HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIO_I2C_EV, 0);
     HAL_NVIC_EnableIRQ(i2cHardwareMap[device].ev_irq);
     state->initialised = true;
 }
diff --git a/src/main/drivers/compass/compass_mpu9250.c b/src/main/drivers/compass/compass_mpu9250.c
index 1e7d6aa178..111a899e17 100755
--- a/src/main/drivers/compass/compass_mpu9250.c
+++ b/src/main/drivers/compass/compass_mpu9250.c
@@ -40,7 +40,7 @@
 #include "drivers/compass/compass.h"
 #include "drivers/compass/compass_mpu9250.h"
 
-#if defined(USE_MAG_MPU9250) && defined(USE_GYRO_MPU9250)
+#if defined(USE_MAG_MPU9250) && defined(USE_IMU_MPU9250)
 
 // No separate hardware descriptor needed. Hardware descriptor initialization is handled by GYRO driver
 
diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index 1d5e4c3772..5afe475184 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -192,9 +192,9 @@ int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint
         return -1;
     }
     if (displayAttributesRequireEmulation(instance, attr)) {
-        char ec;
-        if (displayEmulateTextAttributes(instance, &ec, 1, &attr)) {
-            c = ec;
+        char ec[2];
+        if (displayEmulateTextAttributes(instance, ec, 1, &attr)) {
+            c = ec[0];
         }
     }
     instance->posX = x + 1;
@@ -317,4 +317,3 @@ void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable)
     instance->maxChar = 0;
     displayUpdateMaxChar(instance);
 }
-
diff --git a/src/main/drivers/dma_stm32f3xx.c b/src/main/drivers/dma_stm32f3xx.c
index 8b19d8dbbb..707cf6e097 100644
--- a/src/main/drivers/dma_stm32f3xx.c
+++ b/src/main/drivers/dma_stm32f3xx.c
@@ -98,18 +98,13 @@ void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex)
 
 void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
-
     dmaEnableClock(dma);
 
     dma->irqHandlerCallback = callback;
     dma->userParam = userParam;
 
-    NVIC_InitStructure.NVIC_IRQChannel = dma->irqNumber;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(priority);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(priority);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(dma->irqNumber, priority);
+    NVIC_EnableIRQ(dma->irqNumber);
 }
 
 DMA_t dmaGetByRef(const DMA_Channel_TypeDef * ref)
diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c
index 85393a99bd..955c70f3fe 100644
--- a/src/main/drivers/dma_stm32f4xx.c
+++ b/src/main/drivers/dma_stm32f4xx.c
@@ -106,18 +106,13 @@ void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex)
 
 void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
-
     dmaEnableClock(dma);
 
     dma->irqHandlerCallback = callback;
     dma->userParam = userParam;
 
-    NVIC_InitStructure.NVIC_IRQChannel = dma->irqNumber;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(priority);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(priority);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(dma->irqNumber, priority);
+    NVIC_EnableIRQ(dma->irqNumber);
 }
 
 uint32_t dmaGetChannelByTag(dmaTag_t tag)
diff --git a/src/main/drivers/dma_stm32f7xx.c b/src/main/drivers/dma_stm32f7xx.c
index b044cc497f..fd8b056c28 100644
--- a/src/main/drivers/dma_stm32f7xx.c
+++ b/src/main/drivers/dma_stm32f7xx.c
@@ -110,7 +110,7 @@ void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t prior
     dma->irqHandlerCallback = callback;
     dma->userParam = userParam;
 
-    HAL_NVIC_SetPriority(dma->irqNumber, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority));
+    HAL_NVIC_SetPriority(dma->irqNumber, priority, 0);
     HAL_NVIC_EnableIRQ(dma->irqNumber);
 }
 
diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c
index 16ed467254..9185257e95 100644
--- a/src/main/drivers/exti.c
+++ b/src/main/drivers/exti.c
@@ -90,7 +90,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t conf
 
     if (extiGroupPriority[group] > irqPriority) {
         extiGroupPriority[group] = irqPriority;
-        HAL_NVIC_SetPriority(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+        HAL_NVIC_SetPriority(extiGroupIRQn[group], irqPriority, 0);
         HAL_NVIC_EnableIRQ(extiGroupIRQn[group]);
     }
 }
@@ -131,12 +131,8 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ
     if (extiGroupPriority[group] > irqPriority) {
         extiGroupPriority[group] = irqPriority;
 
-        NVIC_InitTypeDef NVIC_InitStructure;
-        NVIC_InitStructure.NVIC_IRQChannel = extiGroupIRQn[group];
-        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority);
-        NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority);
-        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-        NVIC_Init(&NVIC_InitStructure);
+        NVIC_SetPriority(extiGroupIRQn[group], irqPriority);
+        NVIC_EnableIRQ(extiGroupIRQn[group]);
     }
 }
 #endif
diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c
new file mode 100644
index 0000000000..67669ade06
--- /dev/null
+++ b/src/main/drivers/flash.c
@@ -0,0 +1,292 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav is free software. You can redistribute
+ * this software and/or modify this software 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 this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#ifdef USE_FLASHFS
+
+#include "flash.h"
+#include "flash_m25p16.h"
+#include "drivers/bus_spi.h"
+#include "drivers/io.h"
+#include "drivers/time.h"
+
+static flashPartitionTable_t flashPartitionTable;
+static int flashPartitions = 0;
+
+#ifdef USE_SPI
+static bool flashSpiInit(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_init(0);
+#endif
+    return false;
+}
+#endif // USE_SPI
+
+bool flashDeviceInit(void)
+{
+#ifdef USE_SPI
+    return flashSpiInit();
+#endif
+
+    return false;
+}
+
+bool flashIsReady(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_isReady();
+#endif
+    return false;
+}
+
+bool flashWaitForReady(uint32_t timeoutMillis)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_waitForReady(timeoutMillis);
+#endif
+    return false;
+}
+
+void flashEraseSector(uint32_t address)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_eraseSector(address);
+#endif
+}
+
+void flashEraseCompletely(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_eraseCompletely();
+#endif
+}
+
+#if 0
+void flashPageProgramBegin(uint32_t address)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_pageProgramBegin(address);
+#endif
+}
+
+void flashPageProgramContinue(const uint8_t *data, int length)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_pageProgramContinue(data, length);
+#endif
+}
+
+void flashPageProgramFinish(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_pageProgramFinish();
+#endif
+}
+#endif
+
+uint32_t flashPageProgram(uint32_t address, const uint8_t *data, int length)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_pageProgram(address, data, length);
+#endif
+}
+
+int flashReadBytes(uint32_t address, uint8_t *buffer, int length)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_readBytes(address, buffer, length);
+#endif
+    return 0;
+}
+
+void flashFlush(void)
+{
+}
+
+const flashGeometry_t *flashGetGeometry(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_getGeometry();
+#endif
+
+    return NULL;
+}
+
+/*
+ * Flash partitioning
+ *
+ * Partition table is not currently stored on the flash, in-memory only.
+ *
+ * Partitions are required so that Badblock management (inc spare blocks), FlashFS (Blackbox Logging), Configuration and Firmware can be kept separate and tracked.
+ *
+ * XXX FIXME
+ * XXX Note that Flash FS must start at sector 0.
+ * XXX There is existing blackbox/flash FS code the relies on this!!!
+ * XXX This restriction can and will be fixed by creating a set of flash operation functions that take partition as an additional parameter.
+ */
+
+static __attribute__((unused)) void createPartition(flashPartitionType_e type, uint32_t size, flashSector_t *endSector)
+{
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    flashSector_t partitionSectors = (size / flashGeometry->sectorSize);
+
+    if (size % flashGeometry->sectorSize > 0) {
+        partitionSectors++; // needs a portion of a sector.
+    }
+
+    flashSector_t startSector = (*endSector + 1) - partitionSectors; // + 1 for inclusive
+
+    flashPartitionSet(type, startSector, *endSector);
+
+    *endSector = startSector - 1;
+}
+
+static void flashConfigurePartitions(void)
+{
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    if (flashGeometry->totalSize == 0) {
+        return;
+    }
+
+    flashSector_t startSector = 0;
+    flashSector_t endSector = flashGeometry->sectors - 1; // 0 based index
+
+    const flashPartition_t *badBlockPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT);
+    if (badBlockPartition) {
+        endSector = badBlockPartition->startSector - 1;
+    }
+
+#if defined(FIRMWARE_SIZE)
+    createPartition(FLASH_PARTITION_TYPE_FIRMWARE, FIRMWARE_SIZE*1024, &endSector);
+#endif
+
+#if defined(MSP_FIRMWARE_UPDATE)
+    createPartition(FLASH_PARTITION_TYPE_FIRMWARE_UPDATE_META, flashGeometry->sectorSize, &endSector);
+    createPartition(FLASH_PARTITION_TYPE_UPDATE_FIRMWARE, FLASH_SIZE*1024, &endSector);
+    createPartition(FLASH_PARTITION_TYPE_FULL_BACKUP, FLASH_SIZE*1024, &endSector);
+#endif
+
+#if defined(CONFIG_IN_EXTERNAL_FLASH)
+    createPartition(FLASH_PARTITION_TYPE_CONFIG, EEPROM_SIZE, &endSector);
+#endif
+
+#ifdef USE_FLASHFS
+    flashPartitionSet(FLASH_PARTITION_TYPE_FLASHFS, startSector, endSector);
+#endif
+}
+
+flashPartition_t *flashPartitionFindByType(uint8_t type)
+{
+    for (int index = 0; index < FLASH_MAX_PARTITIONS; index++) {
+        flashPartition_t *candidate = &flashPartitionTable.partitions[index];
+        if (candidate->type == type) {
+            return candidate;
+        }
+    }
+
+    return NULL;
+}
+
+const flashPartition_t *flashPartitionFindByIndex(uint8_t index)
+{
+    if (index >= flashPartitions) {
+        return NULL;
+    }
+
+    return &flashPartitionTable.partitions[index];
+}
+
+void flashPartitionSet(uint8_t type, uint32_t startSector, uint32_t endSector)
+{
+    flashPartition_t *entry = flashPartitionFindByType(type);
+
+    if (!entry) {
+        if (flashPartitions == FLASH_MAX_PARTITIONS - 1) {
+            return;
+        }
+        entry = &flashPartitionTable.partitions[flashPartitions++];
+    }
+
+    entry->type = type;
+    entry->startSector = startSector;
+    entry->endSector = endSector;
+}
+
+// Must be in sync with FLASH_PARTITION_TYPE
+static const char *flashPartitionNames[] = {
+    "UNKNOWN  ",
+    "PARTITION",
+    "FLASHFS  ",
+    "BBMGMT   ",
+    "FIRMWARE ",
+    "CONFIG   ",
+    "FW UPDT  ",
+};
+
+const char *flashPartitionGetTypeName(flashPartitionType_e type)
+{
+    if (type < ARRAYLEN(flashPartitionNames)) {
+        return flashPartitionNames[type];
+    }
+
+    return NULL;
+}
+
+bool flashInit(void)
+{
+    memset(&flashPartitionTable, 0x00, sizeof(flashPartitionTable));
+
+    bool haveFlash = flashDeviceInit();
+
+    flashConfigurePartitions();
+
+    return haveFlash;
+}
+
+int flashPartitionCount(void)
+{
+    return flashPartitions;
+}
+
+uint32_t flashPartitionSize(flashPartition_t *partition)
+{
+    const flashGeometry_t * const geometry = flashGetGeometry();
+    return (partition->endSector - partition->startSector + 1) * geometry->sectorSize;
+}
+
+void flashPartitionErase(flashPartition_t *partition)
+{
+    const flashGeometry_t * const geometry = flashGetGeometry();
+
+    for (unsigned i = partition->startSector; i <= partition->endSector; i++) {
+        uint32_t flashAddress = geometry->sectorSize * i;
+        flashEraseSector(flashAddress);
+        flashWaitForReady(0);
+    }
+}
+#endif // USE_FLASH_CHIP
diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h
index 21216825c1..2a608fcb4d 100644
--- a/src/main/drivers/flash.h
+++ b/src/main/drivers/flash.h
@@ -1,31 +1,94 @@
 /*
- * This file is part of Cleanflight.
+ * This file is part of iNav.
  *
- * 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.
+ * iNav is free software. You can redistribute
+ * this software and/or modify this software 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.
+ * 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
  */
 
 #pragma once
 
 #include <stdint.h>
 
+//#include "drivers/io.h"
+
+//#ifdef USE_FLASHFS
+
+typedef uint16_t flashSector_t;
+
 typedef struct flashGeometry_s {
-    uint16_t sectors; // Count of the number of erasable blocks on the device
-
-    uint16_t pagesPerSector;
-    const uint16_t pageSize; // In bytes
-
+    flashSector_t sectors; // Count of the number of erasable blocks on the device
+    uint16_t pageSize; // In bytes
     uint32_t sectorSize; // This is just pagesPerSector * pageSize
-
     uint32_t totalSize;  // This is just sectorSize * sectors
+    uint16_t pagesPerSector;
 } flashGeometry_t;
+
+bool flashInit(void);
+
+bool flashIsReady(void);
+bool flashWaitForReady(uint32_t timeoutMillis);
+void flashEraseSector(uint32_t address);
+void flashEraseCompletely(void);
+#if 0
+void flashPageProgramBegin(uint32_t address);
+void flashPageProgramContinue(const uint8_t *data, int length);
+void flashPageProgramFinish(void);
+#endif
+uint32_t flashPageProgram(uint32_t address, const uint8_t *data, int length);
+int flashReadBytes(uint32_t address, uint8_t *buffer, int length);
+void flashFlush(void);
+const flashGeometry_t *flashGetGeometry(void);
+
+//
+// flash partitioning api
+//
+
+typedef struct flashPartition_s {
+    uint8_t type;
+    flashSector_t startSector;
+    flashSector_t endSector;
+} flashPartition_t;
+
+#define FLASH_PARTITION_SECTOR_COUNT(partition) (partition->endSector + 1 - partition->startSector) // + 1 for inclusive, start and end sector can be the same sector.
+
+// Must be in sync with flashPartitionTypeNames[]
+// Should not be deleted or reordered once the code is writing a table to a flash.
+typedef enum {
+    FLASH_PARTITION_TYPE_UNKNOWN = 0,
+    FLASH_PARTITION_TYPE_PARTITION_TABLE,
+    FLASH_PARTITION_TYPE_FLASHFS,
+    FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT,
+    FLASH_PARTITION_TYPE_FIRMWARE,
+    FLASH_PARTITION_TYPE_CONFIG,
+    FLASH_PARTITION_TYPE_FULL_BACKUP,
+    FLASH_PARTITION_TYPE_FIRMWARE_UPDATE_META,
+    FLASH_PARTITION_TYPE_UPDATE_FIRMWARE,
+    FLASH_MAX_PARTITIONS
+} flashPartitionType_e;
+
+typedef struct flashPartitionTable_s {
+    flashPartition_t partitions[FLASH_MAX_PARTITIONS];
+} flashPartitionTable_t;
+
+void flashPartitionSet(uint8_t index, uint32_t startSector, uint32_t endSector);
+flashPartition_t *flashPartitionFindByType(flashPartitionType_e type);
+const flashPartition_t *flashPartitionFindByIndex(uint8_t index);
+const char *flashPartitionGetTypeName(flashPartitionType_e type);
+int flashPartitionCount(void);
+uint32_t flashPartitionSize(flashPartition_t *partition);
+void flashPartitionErase(flashPartition_t *partition);
+
+//#endif [> USE_FLASHFS <]
diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c
index 35d0a75505..c2529b436b 100644
--- a/src/main/drivers/flash_m25p16.c
+++ b/src/main/drivers/flash_m25p16.c
@@ -118,7 +118,7 @@ bool m25p16_waitForReady(uint32_t timeoutMillis)
 {
     uint32_t time = millis();
     while (!m25p16_isReady()) {
-        if (millis() - time > timeoutMillis) {
+        if (timeoutMillis && (millis() - time > timeoutMillis)) {
             return false;
         }
     }
diff --git a/src/main/drivers/irlock.c b/src/main/drivers/irlock.c
new file mode 100644
index 0000000000..179fe30c61
--- /dev/null
+++ b/src/main/drivers/irlock.c
@@ -0,0 +1,88 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+#include <math.h>
+
+#include "platform.h"
+
+#include "drivers/sensor.h"
+#include "drivers/irlock.h"
+#include "drivers/time.h"
+
+#define IRLOCK_OBJECT_SYNC ((uint16_t)0xaa55)
+#define IRLOCK_FRAME_SYNC ((uint32_t)(IRLOCK_OBJECT_SYNC | (IRLOCK_OBJECT_SYNC << 16)))
+
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+static bool irlockHealthy = false;
+
+static bool irlockFrameSync(irlockDev_t *irlockDev)
+{
+    uint32_t sync_word = 0;
+    uint8_t count = 10;
+    while (count-- && sync_word != IRLOCK_FRAME_SYNC) {
+        uint8_t sync_byte;
+        irlockHealthy = busRead(irlockDev->busDev, 0xFF, &sync_byte);
+        if (!(irlockHealthy && sync_byte)) return false;
+        sync_word = (sync_word >> 8) | (((uint32_t)sync_byte) << 24);
+    }
+    return sync_word == IRLOCK_FRAME_SYNC;
+}
+
+static bool irlockRead(irlockDev_t *irlockDev, irlockData_t *irlockData)
+{
+    if (irlockFrameSync(irlockDev) && busReadBuf(irlockDev->busDev, 0xFF, (void*)irlockData, sizeof(*irlockData))) {
+        uint16_t cksum = irlockData->signature + irlockData->posX + irlockData->posY + irlockData->sizeX + irlockData->sizeY;
+        if (irlockData->cksum == cksum) return true;
+    }
+    return false;
+}
+
+static bool deviceDetect(irlockDev_t *irlockDev)
+{
+    uint8_t buf;
+    bool detected = busRead(irlockDev->busDev, 0xFF, &buf);
+    return !!detected;
+}
+
+bool irlockDetect(irlockDev_t *irlockDev)
+{
+    irlockDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_IRLOCK, 0, OWNER_IRLOCK);
+    if (irlockDev->busDev == NULL) {
+        return false;
+    }
+
+    if (!deviceDetect(irlockDev)) {
+        busDeviceDeInit(irlockDev->busDev);
+        return false;
+    }
+
+    irlockDev->read = irlockRead;
+
+    return true;
+}
+
+bool irlockIsHealthy(void)
+{
+    return irlockHealthy;
+}
+
+#endif /* USE_IRLOCK */
diff --git a/src/main/drivers/irlock.h b/src/main/drivers/irlock.h
new file mode 100644
index 0000000000..a91a39552f
--- /dev/null
+++ b/src/main/drivers/irlock.h
@@ -0,0 +1,45 @@
+/*
+ * 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
+
+#include "drivers/sensor.h"
+#include "drivers/io_types.h"
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+#define IRLOCK_RES_X 320
+#define IRLOCK_RES_Y 200
+
+typedef struct {
+    uint16_t cksum;
+    uint16_t signature;
+    uint16_t posX;
+    uint16_t posY;
+    uint16_t sizeX;
+    uint16_t sizeY;
+} irlockData_t;
+
+typedef struct irlockDev_s {
+    busDevice_t *busDev;
+    bool (*read)(struct irlockDev_s *irlockDev, irlockData_t *irlockData);
+} irlockDev_t;
+
+bool irlockDetect(irlockDev_t *irlockDev);
+bool irlockIsHealthy(void);
+
+#endif /* USE_IRLOCK */
diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h
index 52aa554e80..68586b7c08 100644
--- a/src/main/drivers/nvic.h
+++ b/src/main/drivers/nvic.h
@@ -2,43 +2,25 @@
 #pragma once
 
 
-// can't use 0
-#define NVIC_PRIO_MAX                      NVIC_BUILD_PRIORITY(0, 1)
-#define NVIC_PRIO_TIMER                    NVIC_BUILD_PRIORITY(1, 1)
-#define NVIC_PRIO_BARO_EXTI                NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_SONAR_EXTI               NVIC_BUILD_PRIORITY(2, 0)  // maybe increate slightly
-#define NVIC_PRIO_TRANSPONDER_DMA          NVIC_BUILD_PRIORITY(3, 0)
-#define NVIC_PRIO_GYRO_INT_EXTI             NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_MAG_INT_EXTI             NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_WS2811_DMA               NVIC_BUILD_PRIORITY(1, 2)  // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?)
-#define NVIC_PRIO_SERIALUART1              NVIC_BUILD_PRIORITY(1, 1)
-#define NVIC_PRIO_SERIALUART2              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART3              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART4              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART5              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART6              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART7              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART8              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_I2C_ER                   NVIC_BUILD_PRIORITY(0, 0)
-#define NVIC_PRIO_I2C_EV                   NVIC_BUILD_PRIORITY(0, 0)
-#define NVIC_PRIO_USB                      NVIC_BUILD_PRIORITY(2, 0)
-#define NVIC_PRIO_USB_WUP                  NVIC_BUILD_PRIORITY(1, 0)
-#define NVIC_PRIO_SONAR_ECHO               NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_MPU_DATA_READY           NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_MAG_DATA_READY           NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_CALLBACK                 NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_MAX7456_DMA              NVIC_BUILD_PRIORITY(3, 0)
+// NVIC_SetPriority expects priority encoded according to priority grouping
+// We allocate zero bits for sub-priority, therefore we have 16 priorities to use on STM32
 
+// can't use 0
+#define NVIC_PRIO_MAX                       1
+#define NVIC_PRIO_I2C_ER                    2
+#define NVIC_PRIO_I2C_EV                    2
+#define NVIC_PRIO_TIMER                     3
+#define NVIC_PRIO_TIMER_DMA                 3
+#define NVIC_PRIO_SDIO                      3
+#define NVIC_PRIO_GYRO_INT_EXTI             4
+#define NVIC_PRIO_USB                       5
+#define NVIC_PRIO_SERIALUART                5
+#define NVIC_PRIO_SONAR_EXTI                7
+
+
+// Use all available bits for priority and zero bits to sub-priority
 #ifdef USE_HAL_DRIVER
-// utility macros to join/split priority
-#define NVIC_PRIORITY_GROUPING NVIC_PRIORITYGROUP_2
-#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING)))))<<4)&0xf0)
-#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING))))>>4)
-#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING))))>>4)
+#define NVIC_PRIORITY_GROUPING NVIC_PRIORITYGROUP_4
 #else
-// utility macros to join/split priority
-#define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_2
-#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING>>8))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8)))))<<4)&0xf0)
-#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
-#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
+#define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_4
 #endif
diff --git a/src/main/drivers/osd.c b/src/main/drivers/osd.c
index 418758f61e..231d8eec28 100644
--- a/src/main/drivers/osd.c
+++ b/src/main/drivers/osd.c
@@ -56,12 +56,12 @@ static void osdGridBufferConstrainRect(int *x, int *y, int *w, int *h, int total
         *y = 0;
     }
     int maxX = *x + *w;
-    int extraWidth = maxX - totalWidth;
+    int extraWidth = maxX - totalWidth + 1;
     if (extraWidth > 0) {
         *w -= extraWidth;
     }
     int maxY = *y + *h;
-    int extraHeight = maxY - totalHeight;
+    int extraHeight = maxY - totalHeight + 1;
     if (extraHeight > 0) {
         *h -= extraHeight;
     }
diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h
index cd9bb83258..0b08e7b51d 100644
--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@@ -117,7 +117,7 @@
 #define SYM_3D_MPH                0x8A // 138 MPH 3D
 
 #define SYM_RPM                   0x8B // 139 RPM
-//                                0x8C // 140 -
+#define SYM_WAYPOINT              0x8C // 140 Waypoint
 //                                0x8D // 141 -
 //                                0x8E // 142 -
 //                                0x8F // 143 -
@@ -205,6 +205,7 @@
 #define SYM_BARO_TEMP             0xF0 // 240
 #define SYM_IMU_TEMP              0xF1 // 241
 #define SYM_TEMP                  0xF2 // 242
+#define SYM_ESC_TEMP              0xF3 // 243
 
 #define SYM_TEMP_SENSOR_FIRST     0xF2 // 242
 #define SYM_TEMP_SENSOR_LAST      0xF7 // 247
diff --git a/src/main/drivers/persistent.c b/src/main/drivers/persistent.c
new file mode 100644
index 0000000000..b4ed38903a
--- /dev/null
+++ b/src/main/drivers/persistent.c
@@ -0,0 +1,119 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav free software. You can redistribute
+ * this software and/or modify this software 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 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * An implementation of persistent data storage utilizing RTC backup data register.
+ * Retains values written across software resets and boot loader activities.
+ */
+
+#include <stdint.h>
+#include "platform.h"
+
+#include "drivers/persistent.h"
+#include "drivers/system.h"
+
+#define PERSISTENT_OBJECT_MAGIC_VALUE (('i' << 24)|('N' << 16)|('a' << 8)|('v' << 0))
+
+#ifdef USE_HAL_DRIVER
+
+uint32_t persistentObjectRead(persistentObjectId_e id)
+{
+    RTC_HandleTypeDef rtcHandle = { .Instance = RTC };
+
+    uint32_t value = HAL_RTCEx_BKUPRead(&rtcHandle, id);
+
+    return value;
+}
+
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value)
+{
+    RTC_HandleTypeDef rtcHandle = { .Instance = RTC };
+
+    HAL_RTCEx_BKUPWrite(&rtcHandle, id, value);
+}
+
+void persistentObjectRTCEnable(void)
+{
+    RTC_HandleTypeDef rtcHandle = { .Instance = RTC };
+
+#if !defined(STM32H7)
+    __HAL_RCC_PWR_CLK_ENABLE(); // Enable Access to PWR
+#endif
+    HAL_PWR_EnableBkUpAccess(); // Disable backup domain protection
+
+#if defined(__HAL_RCC_RTC_CLK_ENABLE)
+    // For those MCUs with RTCAPBEN bit in RCC clock enable register, turn it on.
+    __HAL_RCC_RTC_CLK_ENABLE(); // Enable RTC module
+#endif
+
+    // We don't need a clock source for RTC itself. Skip it.
+
+    __HAL_RTC_WRITEPROTECTION_ENABLE(&rtcHandle);  // Reset sequence
+    __HAL_RTC_WRITEPROTECTION_DISABLE(&rtcHandle); // Apply sequence
+}
+
+#else
+uint32_t persistentObjectRead(persistentObjectId_e id)
+{
+    uint32_t value = RTC_ReadBackupRegister(id);
+
+    return value;
+}
+
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value)
+{
+    RTC_WriteBackupRegister(id, value);
+}
+
+void persistentObjectRTCEnable(void)
+{
+    RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); // Enable Access to PWR
+    PWR_BackupAccessCmd(ENABLE); // Disable backup domain protection
+
+    // We don't need a clock source for RTC itself. Skip it.
+
+    RTC_WriteProtectionCmd(ENABLE);  // Reset sequence
+    RTC_WriteProtectionCmd(DISABLE); // Apply sequence
+}
+#endif
+
+void persistentObjectInit(void)
+{
+    // Configure and enable RTC for backup register access
+
+    persistentObjectRTCEnable();
+
+    // XXX Magic value checking may be sufficient
+
+    uint32_t wasSoftReset;
+
+#ifdef STM32H7
+    wasSoftReset = RCC->RSR & RCC_RSR_SFTRSTF;
+#else
+    wasSoftReset = RCC->CSR & RCC_CSR_SFTRSTF;
+#endif
+
+    if (!wasSoftReset || (persistentObjectRead(PERSISTENT_OBJECT_MAGIC) != PERSISTENT_OBJECT_MAGIC_VALUE)) {
+        for (int i = 1; i < PERSISTENT_OBJECT_COUNT; i++) {
+            persistentObjectWrite(i, 0);
+        }
+        persistentObjectWrite(PERSISTENT_OBJECT_MAGIC, PERSISTENT_OBJECT_MAGIC_VALUE);
+    }
+}
diff --git a/src/main/drivers/persistent.h b/src/main/drivers/persistent.h
new file mode 100644
index 0000000000..ff26802788
--- /dev/null
+++ b/src/main/drivers/persistent.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+// Available RTC backup registers (4-byte words) per MCU type
+// F4: 20 words
+// F7: 32 words
+// H7: 32 words
+
+typedef enum {
+    PERSISTENT_OBJECT_MAGIC = 0,
+    PERSISTENT_OBJECT_RESET_REASON,
+    PERSISTENT_OBJECT_COUNT,
+} persistentObjectId_e;
+
+// Values for PERSISTENT_OBJECT_RESET_REASON
+#define RESET_NONE                                      0
+#define RESET_BOOTLOADER_REQUEST_ROM                    1
+#define RESET_MSC_REQUEST                               2  // MSC invocation was requested
+
+void persistentObjectInit(void);
+uint32_t persistentObjectRead(persistentObjectId_e id);
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value);
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 00e1f43489..8c6eaea463 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -268,7 +268,7 @@ static bool motorsUseHardwareTimers(void)
 
 static bool servosUseHardwareTimers(void)
 {
-    return !feature(FEATURE_PWM_SERVO_DRIVER);
+    return servoConfig()->servo_protocol == SERVO_TYPE_PWM;
 }
 
 static void pwmInitMotors(timMotorServoHardware_t * timOutputs)
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index cb4d760a05..2c8b8f83a5 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -49,6 +49,12 @@ typedef enum {
     PWM_TYPE_SERIALSHOT,
 } motorPwmProtocolTypes_e;
 
+typedef enum {
+    SERVO_TYPE_PWM = 0,
+    SERVO_TYPE_SERVO_DRIVER,
+    SERVO_TYPE_SBUS
+} servoProtocolType_e;
+
 typedef enum {
     PWM_INIT_ERROR_NONE = 0,
     PWM_INIT_ERROR_TOO_MANY_MOTORS,
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 15a68faa19..6472741a78 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "build/debug.h"
 
 #include "common/log.h"
@@ -35,6 +37,7 @@
 
 #include "io/pwmdriver_i2c.h"
 #include "io/esc_serialshot.h"
+#include "io/servo_sbus.h"
 #include "sensors/esc_sensor.h"
 
 #include "config/feature.h"
@@ -325,6 +328,10 @@ bool isMotorProtocolDigital(void)
 
 void pwmRequestMotorTelemetry(int motorIndex)
 {
+    if (!isMotorProtocolDigital()) {
+        return;
+    }
+
     const int motorCount = getMotorCount();
     for (int index = 0; index < motorCount; index++) {
         if (motors[index].pwmPort && motors[index].pwmPort->configured && index == motorIndex) {
@@ -343,10 +350,6 @@ void pwmCompleteMotorUpdate(void)
     int motorCount = getMotorCount();
     timeUs_t currentTimeUs = micros();
 
-#ifdef USE_ESC_SENSOR
-    escSensorUpdate(currentTimeUs);
-#endif
-
     // Enforce motor update rate
     if ((digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) {
         return;
@@ -385,6 +388,15 @@ void pwmCompleteMotorUpdate(void)
     }
 #endif
 }
+
+#else // digital motor protocol
+
+// This stub is needed to avoid ESC_SENSOR dependency on DSHOT
+void pwmRequestMotorTelemetry(int motorIndex)
+{
+    UNUSED(motorIndex);
+}
+
 #endif
 
 void pwmMotorPreconfigure(void)
@@ -415,10 +427,6 @@ void pwmMotorPreconfigure(void)
         case PWM_TYPE_DSHOT600:
         case PWM_TYPE_DSHOT300:
         case PWM_TYPE_DSHOT150:
-#ifdef USE_ESC_SENSOR
-            // DSHOT supports a dedicated wire ESC telemetry. Kick off the ESC-sensor receiver initialization
-            escSensorInitialize();
-#endif
             motorConfigDigitalUpdateInterval(motorConfig()->motorPwmRate);
             motorWritePtr = pwmWriteDigital;
             break;
@@ -505,14 +513,27 @@ static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value)
 
 void pwmServoPreconfigure(void)
 {
-    servoWritePtr = pwmServoWriteStandard;
+    // Protocol-specific configuration
+    switch (servoConfig()->servo_protocol) {
+        default:
+        case SERVO_TYPE_PWM:
+            servoWritePtr = pwmServoWriteStandard;
+            break;
 
 #ifdef USE_PWM_SERVO_DRIVER
-    // If PCA9685 is enabled - switch the servo write function to external
-    if (feature(FEATURE_PWM_SERVO_DRIVER)) {
-        servoWritePtr = pwmServoWriteExternalDriver;
-    }
+        case SERVO_TYPE_SERVO_DRIVER:
+            pwmDriverInitialize();
+            servoWritePtr = pwmServoWriteExternalDriver;
+            break;
 #endif
+
+#ifdef USE_SERVO_SBUS
+        case SERVO_TYPE_SBUS:
+            sbusServoInitialize();
+            servoWritePtr = sbusServoUpdate;
+            break;
+#endif
+    }
 }
 
 bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)
diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c
index 1169f15290..0f12ce3bc9 100644
--- a/src/main/drivers/resource.c
+++ b/src/main/drivers/resource.c
@@ -22,7 +22,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
     "RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
     "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER",
     "NRF24", "VTX", "SPI_PREINIT", "COMPASS", "TEMPERATURE", "1-WIRE", "AIRSPEED", "OLED DISPLAY",
-    "PINIO"
+    "PINIO", "IRLOCK"
 };
 
 const char * const resourceNames[RESOURCE_TOTAL_COUNT] = {
diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h
index fbbbbdba27..67153a61f2 100644
--- a/src/main/drivers/resource.h
+++ b/src/main/drivers/resource.h
@@ -56,6 +56,7 @@ typedef enum {
     OWNER_AIRSPEED,
     OWNER_OLED_DISPLAY,
     OWNER_PINIO,
+    OWNER_IRLOCK,
     OWNER_TOTAL_COUNT
 } resourceOwner_e;
 
diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c
index 707b5e83d9..2734de00ad 100644
--- a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c
+++ b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c
@@ -1411,12 +1411,8 @@ bool SD_Initialize_LL(DMA_Stream_TypeDef *dmaRef)
     IOConfigGPIOAF(cmd, SDIO_CMD, GPIO_AF_SDIO);
 
     // NVIC configuration for SDIO interrupts
-    NVIC_InitTypeDef NVIC_InitStructure;
-    NVIC_InitStructure.NVIC_IRQChannel = SDIO_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(1);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(0);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(SDIO_IRQn, NVIC_PRIO_SDIO);
+    NVIC_EnableIRQ(SDIO_IRQn);
 
     dma_stream = dmaRef;
     RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c
index 2c941f2ced..d5b11946b2 100644
--- a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c
+++ b/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c
@@ -1329,10 +1329,8 @@ bool SD_Initialize_LL(DMA_Stream_TypeDef * dmaRef)
     IOInit(cmd, OWNER_SDCARD, RESOURCE_NONE, 0);
     IOConfigGPIOAF(cmd, SDMMC_CMD, GPIO_AF12_SDMMC1);
 
-    uint32_t PriorityGroup = NVIC_GetPriorityGrouping();
-
     // NVIC configuration for SDIO interrupts
-    NVIC_SetPriority(SDMMC1_IRQn, NVIC_EncodePriority(PriorityGroup, 1, 0));
+    NVIC_SetPriority(SDMMC1_IRQn, NVIC_PRIO_SDIO);
     NVIC_EnableIRQ(SDMMC1_IRQn);
 
     dma_stream = dmaRef;
diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c
index b8d79fd214..5f944fb84d 100644
--- a/src/main/drivers/serial_uart_stm32f30x.c
+++ b/src/main/drivers/serial_uart_stm32f30x.c
@@ -169,7 +169,6 @@ void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, ui
 #ifdef USE_UART1
 uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t options)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
     uartPort_t *s;
 
     static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE];
@@ -191,11 +190,8 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART1_TX_PIN)), IOGetByTag(IO_TAG(UART1_RX_PIN)), mode, options, GPIO_AF_7, 1);
 
-    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(USART1_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(USART1_IRQn);
 
     return s;
 }
@@ -204,7 +200,6 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option
 #ifdef USE_UART2
 uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t options)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
     uartPort_t *s;
 
     static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE];
@@ -226,11 +221,8 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART2_TX_PIN)), IOGetByTag(IO_TAG(UART2_RX_PIN)), mode, options, GPIO_AF_7, 2);
 
-    NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART2);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART2);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(USART2_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(USART2_IRQn);
 
     return s;
 }
@@ -239,7 +231,6 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option
 #ifdef USE_UART3
 uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t options)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
     uartPort_t *s;
 
     static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE];
@@ -261,11 +252,8 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOGetByTag(IO_TAG(UART3_RX_PIN)), mode, options, GPIO_AF_7, 3);
 
-    NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART3);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART3);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(USART3_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(USART3_IRQn);
 
     return s;
 }
@@ -277,7 +265,6 @@ uartPort_t *serialUART4(uint32_t baudRate, portMode_t mode, portOptions_t option
     uartPort_t *s;
     static volatile uint8_t rx4Buffer[UART4_RX_BUFFER_SIZE];
     static volatile uint8_t tx4Buffer[UART4_TX_BUFFER_SIZE];
-    NVIC_InitTypeDef NVIC_InitStructure;
 
     s = &uartPort4;
     s->port.vTable = uartVTable;
@@ -295,11 +282,8 @@ uartPort_t *serialUART4(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART4_TX_PIN)), IOGetByTag(IO_TAG(UART4_RX_PIN)), mode, options, GPIO_AF_5, 4);
 
-    NVIC_InitStructure.NVIC_IRQChannel = UART4_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART4);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART4);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(UART4_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(UART4_IRQn);
 
     return s;
 }
@@ -311,7 +295,6 @@ uartPort_t *serialUART5(uint32_t baudRate, portMode_t mode, portOptions_t option
     uartPort_t *s;
     static volatile uint8_t rx5Buffer[UART5_RX_BUFFER_SIZE];
     static volatile uint8_t tx5Buffer[UART5_TX_BUFFER_SIZE];
-    NVIC_InitTypeDef NVIC_InitStructure;
 
     s = &uartPort5;
     s->port.vTable = uartVTable;
@@ -329,11 +312,8 @@ uartPort_t *serialUART5(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART5_TX_PIN)), IOGetByTag(IO_TAG(UART5_RX_PIN)), mode, options, GPIO_AF_5, 5);
 
-    NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART5);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART5);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(UART5_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(UART5_IRQn);
 
     return s;
 }
diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c
index dc9f0ddc50..2b310ec575 100644
--- a/src/main/drivers/serial_uart_stm32f4xx.c
+++ b/src/main/drivers/serial_uart_stm32f4xx.c
@@ -60,7 +60,7 @@ static uartDevice_t uart1 =
 #endif
     .rcc_apb2 = RCC_APB2(USART1),
     .irq = USART1_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART1
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -76,7 +76,7 @@ static uartDevice_t uart2 =
 #endif
     .rcc_apb1 = RCC_APB1(USART2),
     .irq = USART2_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART2
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -92,7 +92,7 @@ static uartDevice_t uart3 =
 #endif
     .rcc_apb1 = RCC_APB1(USART3),
     .irq = USART3_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART3
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -108,7 +108,7 @@ static uartDevice_t uart4 =
 #endif
     .rcc_apb1 = RCC_APB1(UART4),
     .irq = UART4_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART4
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -124,7 +124,7 @@ static uartDevice_t uart5 =
 #endif
     .rcc_apb1 = RCC_APB1(UART5),
     .irq = UART5_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART5
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -140,7 +140,7 @@ static uartDevice_t uart6 =
 #endif
     .rcc_apb2 = RCC_APB2(USART6),
     .irq = USART6_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART6
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -153,7 +153,7 @@ static uartDevice_t uart7 =
     .af = GPIO_AF_UART7,
     .rcc_apb1 = RCC_APB1(UART7),
     .irq = UART7_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART7
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -166,7 +166,7 @@ static uartDevice_t uart8 =
     .af = GPIO_AF_UART8,
     .rcc_apb1 = RCC_APB1(UART8),
     .irq = UART8_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART8
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -256,7 +256,6 @@ void uartGetPortPins(UARTDevice_e device, serialPortPins_t * pins)
 uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, portOptions_t options)
 {
     uartPort_t *s;
-    NVIC_InitTypeDef NVIC_InitStructure;
 
     uartDevice_t *uart = uartHardwareMap[device];
     if (!uart) return NULL;
@@ -304,11 +303,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
         }
     }
 
-    NVIC_InitStructure.NVIC_IRQChannel = uart->irq;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(uart->irqPriority);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(uart->irqPriority);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(uart->irq, uart->irqPriority);
+    NVIC_EnableIRQ(uart->irq);
 
     return s;
 }
diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c
index bf22467f24..baab69b087 100644
--- a/src/main/drivers/serial_uart_stm32f7xx.c
+++ b/src/main/drivers/serial_uart_stm32f7xx.c
@@ -64,7 +64,7 @@ static uartDevice_t uart1 =
 #endif
     .rcc_apb2 = RCC_APB2(USART1),
     .irq = USART1_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART1
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -84,7 +84,7 @@ static uartDevice_t uart2 =
 #endif
     .rcc_apb1 = RCC_APB1(USART2),
     .irq = USART2_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART2
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -104,7 +104,7 @@ static uartDevice_t uart3 =
 #endif
     .rcc_apb1 = RCC_APB1(USART3),
     .irq = USART3_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART3
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -124,7 +124,7 @@ static uartDevice_t uart4 =
 #endif
     .rcc_apb1 = RCC_APB1(UART4),
     .irq = UART4_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART4
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -144,7 +144,7 @@ static uartDevice_t uart5 =
 #endif
     .rcc_apb1 = RCC_APB1(UART5),
     .irq = UART5_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART5
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -164,7 +164,7 @@ static uartDevice_t uart6 =
 #endif
     .rcc_apb2 = RCC_APB2(USART6),
     .irq = USART6_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART6
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -184,7 +184,7 @@ static uartDevice_t uart7 =
 #endif
     .rcc_apb1 = RCC_APB1(UART7),
     .irq = UART7_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART7
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 #ifdef USE_UART8
@@ -203,7 +203,7 @@ static uartDevice_t uart8 =
 #endif
     .rcc_apb1 = RCC_APB1(UART8),
     .irq = UART8_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART8
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -372,7 +372,7 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
         }
     }
 
-    HAL_NVIC_SetPriority(uart->irq, NVIC_PRIORITY_BASE(uart->irqPriority), NVIC_PRIORITY_SUB(uart->irqPriority));
+    HAL_NVIC_SetPriority(uart->irq, uart->irqPriority, 0);
     HAL_NVIC_EnableIRQ(uart->irq);
 
     return s;
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index 69bde9c41e..55691bb291 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -20,12 +20,11 @@
 
 #include "platform.h"
 
-#include "drivers/light_led.h"
-#include "sound_beeper.h"
-#include "drivers/nvic.h"
-#include "build/atomic.h"
 #include "build/build_config.h"
 
+#include "drivers/light_led.h"
+#include "drivers/persistent.h"
+#include "drivers/sound_beeper.h"
 #include "drivers/system.h"
 #include "drivers/time.h"
 
@@ -48,17 +47,12 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
     failureMode(FAILURE_DEVELOPER); // EXTI_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
 }
 
-// cycles per microsecond
-STATIC_UNIT_TESTED EXTENDED_FASTRAM  timeUs_t usTicks = 0;
-// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
-STATIC_UNIT_TESTED EXTENDED_FASTRAM volatile timeMs_t sysTickUptime = 0;
-STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
 // cached value of RCC->CSR
 uint32_t cachedRccCsrValue;
 
-#ifndef UNIT_TEST
 void cycleCounterInit(void)
 {
+    extern uint32_t usTicks; // From drivers/time.h
 #if defined(USE_HAL_DRIVER)
     usTicks = HAL_RCC_GetSysClockFreq() / 1000000;
 #else
@@ -72,133 +66,52 @@ void cycleCounterInit(void)
     CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
     DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
 }
-#endif // UNIT_TEST
 
-// SysTick
-
-static EXTENDED_FASTRAM volatile int sysTickPending = 0;
-
-void SysTick_Handler(void)
+static inline void systemDisableAllIRQs(void)
 {
-    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
-        sysTickUptime++;
-        sysTickValStamp = SysTick->VAL;
-        sysTickPending = 0;
-        (void)(SysTick->CTRL);
-    }
-#ifdef USE_HAL_DRIVER
-    // used by the HAL for some timekeeping and timeouts, should always be 1ms
-    HAL_IncTick();
-#endif
-}
-
-uint32_t ticks(void)
-{
-#ifdef UNIT_TEST
-    return 0;
-#else
-    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
-    return DWT->CYCCNT;
-#endif
-}
-
-timeDelta_t ticks_diff_us(uint32_t begin, uint32_t end)
-{
-    return (end - begin) / usTicks;
-}
-
-// Return system uptime in microseconds
-timeUs_t microsISR(void)
-{
-    register uint32_t ms, pending, cycle_cnt;
-
-    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
-        cycle_cnt = SysTick->VAL;
-
-        if (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) {
-            // Update pending.
-            // Record it for multiple calls within the same rollover period
-            // (Will be cleared when serviced).
-            // Note that multiple rollovers are not considered.
-
-            sysTickPending = 1;
-
-            // Read VAL again to ensure the value is read after the rollover.
-
-            cycle_cnt = SysTick->VAL;
-        }
-
-        ms = sysTickUptime;
-        pending = sysTickPending;
-    }
-
-    return ((timeUs_t)(ms + pending) * 1000LL) + (usTicks * 1000LL - (timeUs_t)cycle_cnt) / usTicks;
-}
-
-timeUs_t micros(void)
-{
-    register uint32_t ms, cycle_cnt;
-
-    // Call microsISR() in interrupt and elevated (non-zero) BASEPRI context
-
-#ifndef UNIT_TEST
-    if ((SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) || (__get_BASEPRI())) {
-        return microsISR();
-    }
-#endif
-
-    do {
-        ms = sysTickUptime;
-        cycle_cnt = SysTick->VAL;
-    } while (ms != sysTickUptime || cycle_cnt > sysTickValStamp);
-
-    return ((timeUs_t)ms * 1000LL) + (usTicks * 1000LL - (timeUs_t)cycle_cnt) / usTicks;
-}
-
-// Return system uptime in milliseconds (rollover in 49 days)
-timeMs_t millis(void)
-{
-    return sysTickUptime;
-}
-
-#if 1
-void delayMicroseconds(timeUs_t us)
-{
-    timeUs_t now = micros();
-    while (micros() - now < us);
-}
-#else
-void delayMicroseconds(timeUs_t us)
-{
-    uint32_t elapsed = 0;
-    uint32_t lastCount = SysTick->VAL;
-
-    for (;;) {
-        register uint32_t current_count = SysTick->VAL;
-        timeUs_t elapsed_us;
-
-        // measure the time elapsed since the last time we checked
-        elapsed += current_count - lastCount;
-        lastCount = current_count;
-
-        // convert to microseconds
-        elapsed_us = elapsed / usTicks;
-        if (elapsed_us >= us)
-            break;
-
-        // reduce the delay by the elapsed time
-        us -= elapsed_us;
-
-        // keep fractional microseconds for the next iteration
-        elapsed %= usTicks;
+    // We access CMSIS NVIC registers directly here
+    for (int x = 0; x < 8; x++) {
+        // Mask all IRQs controlled by a ICERx
+        NVIC->ICER[x] = 0xFFFFFFFF;
+        // Clear all pending IRQs controlled by a ICPRx
+        NVIC->ICPR[x] = 0xFFFFFFFF;
     }
 }
-#endif
 
-void delay(timeMs_t ms)
+void systemReset(void)
 {
-    while (ms--)
-        delayMicroseconds(1000);
+    __disable_irq();
+    systemDisableAllIRQs();
+    NVIC_SystemReset();
+}
+
+void systemResetToBootloader(void)
+{
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
+    systemReset();
+}
+
+typedef void resetHandler_t(void);
+
+typedef struct isrVector_s {
+    uint32_t    stackEnd;
+    resetHandler_t *resetHandler;
+} isrVector_t;
+
+
+void checkForBootLoaderRequest(void)
+{
+    uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON);
+
+    if (bootloaderRequest != RESET_BOOTLOADER_REQUEST_ROM) {
+        return;
+    }
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
+    volatile isrVector_t *bootloaderVector = (isrVector_t *)systemBootloaderAddress();
+    __set_MSP(bootloaderVector->stackEnd);
+    bootloaderVector->resetHandler();
+    while (1);
 }
 
 #define SHORT_FLASH_DURATION 50
diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h
index 4931c0d599..87cc140690 100644
--- a/src/main/drivers/system.h
+++ b/src/main/drivers/system.h
@@ -39,6 +39,7 @@ void failureMode(failureMode_e mode);
 // bootloader/IAP
 void systemReset(void);
 void systemResetToBootloader(void);
+uint32_t systemBootloaderAddress(void);
 bool isMPUSoftReset(void);
 void cycleCounterInit(void);
 void checkForBootLoaderRequest(void);
diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c
index a31cb08aa9..0719e576c1 100644
--- a/src/main/drivers/system_stm32f30x.c
+++ b/src/main/drivers/system_stm32f30x.c
@@ -24,42 +24,8 @@
 #include "drivers/nvic.h"
 #include "drivers/system.h"
 
-#define AIRCR_VECTKEY_MASK    ((uint32_t)0x05FA0000)
 void SetSysClock(uint8_t underclock);
 
-inline static void NVIC_DisableAllIRQs(void)
-{
-    // We access CMSIS NVIC registers directly here
-    for (int x = 0; x < 8; x++) {
-        // Mask all IRQs controlled by a ICERx
-        NVIC->ICER[x] = 0xFFFFFFFF;
-        // Clear all pending IRQs controlled by a ICPRx
-        NVIC->ICPR[x] = 0xFFFFFFFF;
-    }
-}
-
-void systemReset(void)
-{
-    // Disable all NVIC interrupts
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-
-    // Generate system reset
-    SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
-}
-
-void systemResetToBootloader(void)
-{
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-
-    *((uint32_t *)0x20009FFC) = 0xDEADBEEF; // 40KB SRAM STM32F30X
-
-    // Generate system reset
-    SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
-}
-
-
 void enableGPIOPowerUsageAndNoiseReductions(void)
 {
     RCC_AHBPeriphClockCmd(
@@ -95,6 +61,11 @@ bool isMPUSoftReset(void)
         return false;
 }
 
+uint32_t systemBootloaderAddress(void)
+{
+    return 0x1FFFD800;
+}
+
 static void systemTimekeepingSetup(void)
 {
     RCC_ClocksTypeDef clocks;
@@ -133,7 +104,3 @@ void systemInit(void)
     // Pre-setup SysTick and system time - final setup is done in systemClockSetup
     systemTimekeepingSetup();
 }
-
-void checkForBootLoaderRequest(void)
-{
-}
diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c
index 7da03ec009..aa98183484 100644
--- a/src/main/drivers/system_stm32f4xx.c
+++ b/src/main/drivers/system_stm32f4xx.c
@@ -29,37 +29,8 @@
 #include "drivers/exti.h"
 
 
-#define AIRCR_VECTKEY_MASK    ((uint32_t)0x05FA0000)
 void SetSysClock(void);
 
-inline static void NVIC_DisableAllIRQs(void)
-{
-    // We access CMSIS NVIC registers directly here
-    for (int x = 0; x < 8; x++) {
-        // Mask all IRQs controlled by a ICERx
-        NVIC->ICER[x] = 0xFFFFFFFF;
-        // Clear all pending IRQs controlled by a ICPRx
-        NVIC->ICPR[x] = 0xFFFFFFFF;
-    }
-}
-
-void systemReset(void)
-{
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-    NVIC_SystemReset();
-}
-
-void systemResetToBootloader(void)
-{
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-
-    *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX
-
-    NVIC_SystemReset();
-}
-
 void enableGPIOPowerUsageAndNoiseReductions(void)
 {
 
@@ -167,6 +138,11 @@ bool isMPUSoftReset(void)
         return false;
 }
 
+uint32_t systemBootloaderAddress(void)
+{
+    return 0x1FFF0000;
+}
+
 void systemClockSetup(uint8_t cpuUnderclock)
 {
     (void)cpuUnderclock;
diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c
index 62ee3b1ae1..9c39be23e8 100644
--- a/src/main/drivers/system_stm32f7xx.c
+++ b/src/main/drivers/system_stm32f7xx.c
@@ -27,38 +27,8 @@
 #include "drivers/nvic.h"
 #include "drivers/system.h"
 
-
-#define AIRCR_VECTKEY_MASK    ((uint32_t)0x05FA0000)
 void SystemClock_Config(void);
 
-inline static void NVIC_DisableAllIRQs(void)
-{
-    // We access CMSIS NVIC registers directly here
-    for (int x = 0; x < 8; x++) {
-        // Mask all IRQs controlled by a ICERx
-        NVIC->ICER[x] = 0xFFFFFFFF;
-        // Clear all pending IRQs controlled by a ICPRx
-        NVIC->ICPR[x] = 0xFFFFFFFF;
-    }
-}
-
-void systemReset(void)
-{
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-    NVIC_SystemReset();
-}
-
-void systemResetToBootloader(void)
-{
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-
-    (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF;   // flag that will be readable after reboot
-
-    NVIC_SystemReset();
-}
-
 void enableGPIOPowerUsageAndNoiseReductions(void)
 {
 
@@ -91,6 +61,11 @@ bool isMPUSoftReset(void)
         return false;
 }
 
+uint32_t systemBootloaderAddress(void)
+{
+    return 0x1FF00000;
+}
+
 void systemClockSetup(uint8_t cpuUnderclock)
 {
     (void)cpuUnderclock;
@@ -128,26 +103,3 @@ void systemInit(void)
 
     HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
 }
-
-void(*bootJump)(void);
-void checkForBootLoaderRequest(void)
-{
-    uint32_t bt;
-    __PWR_CLK_ENABLE();
-    __BKPSRAM_CLK_ENABLE();
-    HAL_PWR_EnableBkUpAccess();
-
-    bt = (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) ;
-    if ( bt == 0xDEADBEEF ) {
-        (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) =  0xCAFEFEED; // Reset our trigger
-
-        void (*SysMemBootJump)(void);
-        __SYSCFG_CLK_ENABLE();
-        SYSCFG->MEMRMP |= SYSCFG_MEM_BOOT_ADD0 ;
-        uint32_t p =  (*((uint32_t *) 0x1ff00000));
-        __set_MSP(p); //Set the main stack pointer to its defualt values
-        SysMemBootJump = (void (*)(void)) (*((uint32_t *) 0x1ff00004)); // Point the PC to the System Memory reset vector (+4)
-        SysMemBootJump();
-        while (1);
-    }
-}
diff --git a/src/main/drivers/time.c b/src/main/drivers/time.c
new file mode 100644
index 0000000000..9fcbc78625
--- /dev/null
+++ b/src/main/drivers/time.c
@@ -0,0 +1,172 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+
+#include "build/atomic.h"
+
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+
+// cycles per microsecond, this is deliberately uint32_t to avoid type conversions
+// This is not static so system.c can set it up for us.
+uint32_t usTicks = 0;
+// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
+STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0;
+STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
+
+// Return system uptime in milliseconds (rollover in 49 days)
+timeMs_t millis(void)
+{
+    return sysTickUptime;
+}
+
+// SysTick
+
+static volatile int sysTickPending = 0;
+
+void SysTick_Handler(void)
+{
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        sysTickUptime++;
+        sysTickValStamp = SysTick->VAL;
+        sysTickPending = 0;
+        (void)(SysTick->CTRL);
+    }
+#ifdef USE_HAL_DRIVER
+    // used by the HAL for some timekeeping and timeouts, should always be 1ms
+    HAL_IncTick();
+#endif
+}
+
+uint32_t ticks(void)
+{
+#ifdef UNIT_TEST
+    return 0;
+#else
+    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+    return DWT->CYCCNT;
+#endif
+}
+
+timeDelta_t ticks_diff_us(uint32_t begin, uint32_t end)
+{
+    return (end - begin) / usTicks;
+}
+
+// Return system uptime in microseconds
+timeUs_t microsISR(void)
+{
+    register uint32_t ms, pending, cycle_cnt;
+
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        cycle_cnt = SysTick->VAL;
+
+        if (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) {
+            // Update pending.
+            // Record it for multiple calls within the same rollover period
+            // (Will be cleared when serviced).
+            // Note that multiple rollovers are not considered.
+
+            sysTickPending = 1;
+
+            // Read VAL again to ensure the value is read after the rollover.
+
+            cycle_cnt = SysTick->VAL;
+        }
+
+        ms = sysTickUptime;
+        pending = sysTickPending;
+    }
+
+    // XXX: Be careful to not trigger 64 bit division
+    const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
+    return ((timeUs_t)(ms + pending) * 1000LL) + ((timeUs_t)partial);
+}
+
+timeUs_t micros(void)
+{
+    register uint32_t ms, cycle_cnt;
+
+    // Call microsISR() in interrupt and elevated (non-zero) BASEPRI context
+
+#ifndef UNIT_TEST
+    if ((SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) || (__get_BASEPRI())) {
+        return microsISR();
+    }
+#endif
+
+    do {
+        ms = sysTickUptime;
+        cycle_cnt = SysTick->VAL;
+    } while (ms != sysTickUptime || cycle_cnt > sysTickValStamp);
+
+    // XXX: Be careful to not trigger 64 bit division
+    const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
+    return ((timeUs_t)ms * 1000LL) + ((timeUs_t)partial);
+}
+
+#if 1
+void delayMicroseconds(timeUs_t us)
+{
+    timeUs_t now = micros();
+    while (micros() - now < us);
+}
+#else
+void delayMicroseconds(timeUs_t us)
+{
+    uint32_t elapsed = 0;
+    uint32_t lastCount = SysTick->VAL;
+
+    for (;;) {
+        register uint32_t current_count = SysTick->VAL;
+        timeUs_t elapsed_us;
+
+        // measure the time elapsed since the last time we checked
+        elapsed += current_count - lastCount;
+        lastCount = current_count;
+
+        // convert to microseconds
+        elapsed_us = elapsed / usTicks;
+        if (elapsed_us >= us)
+            break;
+
+        // reduce the delay by the elapsed time
+        us -= elapsed_us;
+
+        // keep fractional microseconds for the next iteration
+        elapsed %= usTicks;
+    }
+}
+#endif
+
+void delay(timeMs_t ms)
+{
+    while (ms--)
+        delayMicroseconds(1000);
+}
diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index a4eb82941d..9f855217ee 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -62,12 +62,12 @@ void impl_timerInitContext(timHardwareContext_t * timCtx)
 void impl_timerNVICConfigure(TCH_t * tch, int irqPriority)
 {
     if (tch->timCtx->timDef->irq) {
-        HAL_NVIC_SetPriority(tch->timCtx->timDef->irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+        HAL_NVIC_SetPriority(tch->timCtx->timDef->irq, irqPriority, 0);
         HAL_NVIC_EnableIRQ(tch->timCtx->timDef->irq);
     }
 
     if (tch->timCtx->timDef->secondIrq) {
-        HAL_NVIC_SetPriority(tch->timCtx->timDef->secondIrq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+        HAL_NVIC_SetPriority(tch->timCtx->timDef->secondIrq, irqPriority, 0);
         HAL_NVIC_EnableIRQ(tch->timCtx->timDef->secondIrq);
     }
 }
@@ -380,7 +380,7 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
     init.PeriphBurst = LL_DMA_PBURST_SINGLE;
 
     dmaInit(tch->dma, OWNER_TIMER, 0);
-    dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_WS2811_DMA, (uint32_t)tch);
+    dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
 
     LL_DMA_Init(tch->dma->dma, streamLL, &init);
 
diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c
index 4dd428ebc5..599b8b5cb3 100644
--- a/src/main/drivers/timer_impl_stdperiph.c
+++ b/src/main/drivers/timer_impl_stdperiph.c
@@ -44,20 +44,14 @@ void impl_timerInitContext(timHardwareContext_t * timCtx)
 
 void impl_timerNVICConfigure(TCH_t * tch, int irqPriority)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
-
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-
     if (tch->timCtx->timDef->irq) {
-        NVIC_InitStructure.NVIC_IRQChannel = tch->timCtx->timDef->irq;
-        NVIC_Init(&NVIC_InitStructure);
+        NVIC_SetPriority(tch->timCtx->timDef->irq, irqPriority);
+        NVIC_EnableIRQ(tch->timCtx->timDef->irq);
     }
 
     if (tch->timCtx->timDef->secondIrq) {
-        NVIC_InitStructure.NVIC_IRQChannel = tch->timCtx->timDef->secondIrq;
-        NVIC_Init(&NVIC_InitStructure);
+        NVIC_SetPriority(tch->timCtx->timDef->secondIrq, irqPriority);
+        NVIC_EnableIRQ(tch->timCtx->timDef->secondIrq);
     }
 }
 
@@ -311,7 +305,7 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
     TIM_Cmd(timer, ENABLE);
 
     dmaInit(tch->dma, OWNER_TIMER, 0);
-    dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_WS2811_DMA, (uint32_t)tch);
+    dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
 
     DMA_DeInit(tch->dma->ref);
     DMA_Cmd(tch->dma->ref, DISABLE);
diff --git a/src/main/drivers/usb_msc.c b/src/main/drivers/usb_msc.c
new file mode 100644
index 0000000000..937a88fb17
--- /dev/null
+++ b/src/main/drivers/usb_msc.c
@@ -0,0 +1,27 @@
+/*
+ * This file is part of iNav
+ *
+ * iNav is free software. You can redistribute
+ * this software and/or modify this software 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 this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "drivers/persistent.h"
+#include <stdbool.h>
+
+bool mscCheckBoot(void)
+{
+    return (persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON) == RESET_MSC_REQUEST);
+}
diff --git a/src/main/drivers/usb_msc.h b/src/main/drivers/usb_msc.h
index 3995b3c170..01334bbef0 100644
--- a/src/main/drivers/usb_msc.h
+++ b/src/main/drivers/usb_msc.h
@@ -24,8 +24,6 @@
 
 #pragma once
 
-#define MSC_MAGIC 0xDDDD1010
-
 void mscInit(void);
 bool mscCheckBoot(void);
 uint8_t mscStart(void);
diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c
index 2b9a7f847b..8bc9a9ade3 100644
--- a/src/main/drivers/usb_msc_f4xx.c
+++ b/src/main/drivers/usb_msc_f4xx.c
@@ -41,6 +41,7 @@
 #include "drivers/io.h"
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
+#include "drivers/persistent.h"
 #include "drivers/sdmmc_sdio.h"
 #include "drivers/time.h"
 #include "drivers/usb_msc.h"
@@ -110,22 +111,16 @@ uint8_t mscStart(void)
 
     USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &MSC_desc, &USBD_MSC_cb, &USR_cb);
 
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
     // NVIC configuration for SYSTick
     NVIC_DisableIRQ(SysTick_IRQn);
-    NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0));
+    NVIC_SetPriority(SysTick_IRQn, 0);
     NVIC_EnableIRQ(SysTick_IRQn);
 
     return 0;
 }
 
-bool mscCheckBoot(void)
-{
-    if (*((uint32_t *)0x2001FFF0) == MSC_MAGIC) {
-        return true;
-    }
-    return false;
-}
-
 bool mscCheckButton(void)
 {
     bool result = false;
@@ -150,7 +145,6 @@ void mscWaitForButton(void)
     while (true) {
         asm("NOP");
         if (mscCheckButton()) {
-            *((uint32_t *)0x2001FFF0) = 0xFFFFFFFF;
             delay(1);
             NVIC_SystemReset();
         }
diff --git a/src/main/drivers/usb_msc_f7xx.c b/src/main/drivers/usb_msc_f7xx.c
index bf56fd7564..bfaee68bb8 100644
--- a/src/main/drivers/usb_msc_f7xx.c
+++ b/src/main/drivers/usb_msc_f7xx.c
@@ -41,6 +41,7 @@
 #include "drivers/io.h"
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
+#include "drivers/persistent.h"
 #include "drivers/time.h"
 #include "drivers/usb_msc.h"
 
@@ -108,22 +109,16 @@ uint8_t mscStart(void)
 
     USBD_Start(&USBD_Device);
 
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
     // NVIC configuration for SYSTick
     NVIC_DisableIRQ(SysTick_IRQn);
-    NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0));
+    NVIC_SetPriority(SysTick_IRQn, 0);
     NVIC_EnableIRQ(SysTick_IRQn);
 
     return 0;
 }
 
-bool mscCheckBoot(void)
-{
-    if (*((__IO uint32_t *)BKPSRAM_BASE + 16) == MSC_MAGIC) {
-        return true;
-    }
-    return false;
-}
-
 bool mscCheckButton(void)
 {
     bool result = false;
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 35bd228907..41dc46a02e 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -44,6 +44,7 @@ extern uint8_t __config_end;
 #include "common/time.h"
 #include "common/typeconversion.h"
 #include "common/global_functions.h"
+#include "common/global_variables.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -55,9 +56,11 @@ extern uint8_t __config_end;
 #include "drivers/buf_writer.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/compass/compass.h"
+#include "drivers/flash.h"
 #include "drivers/io.h"
 #include "drivers/io_impl.h"
 #include "drivers/osd_symbols.h"
+#include "drivers/persistent.h"
 #include "drivers/rx_pwm.h"
 #include "drivers/sdcard/sdcard.h"
 #include "drivers/sensor.h"
@@ -93,6 +96,8 @@ extern uint8_t __config_end;
 #include "io/osd.h"
 #include "io/serial.h"
 
+#include "fc/fc_msp_box.h"
+
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
@@ -150,7 +155,7 @@ static const char * const featureNames[] = {
     "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "",
     "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
     "BLACKBOX", "", "TRANSPONDER", "AIRMODE",
-    "SUPEREXPO", "VTX", "", "", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE",
+    "SUPEREXPO", "VTX", "", "", "", "PWM_OUTPUT_ENABLE",
     "OSD", "FW_LAUNCH", NULL
 };
 
@@ -645,17 +650,19 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi
                 && mac->auxChannelIndex == macDefault->auxChannelIndex
                 && mac->range.startStep == macDefault->range.startStep
                 && mac->range.endStep == macDefault->range.endStep;
+            const box_t *box = findBoxByActiveBoxId(macDefault->modeId);
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
-                macDefault->modeId,
+                box->permanentId,
                 macDefault->auxChannelIndex,
                 MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.startStep),
                 MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep)
             );
         }
+        const box_t *box = findBoxByActiveBoxId(mac->modeId);
         cliDumpPrintLinef(dumpMask, equalsDefault, format,
             i,
-            mac->modeId,
+            box->permanentId,
             mac->auxChannelIndex,
             MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep),
             MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep)
@@ -679,9 +686,12 @@ static void cliAux(char *cmdline)
             ptr = nextArg(ptr);
             if (ptr) {
                 val = fastA2I(ptr);
-                if (val >= 0 && val < CHECKBOX_ITEM_COUNT) {
-                    mac->modeId = val;
-                    validArgumentCount++;
+                if (val >= 0) {
+                    const box_t *box = findBoxByPermanentId(val);
+                    if (box != NULL) {
+                        mac->modeId = box->boxId;
+                        validArgumentCount++;
+                    }
                 }
             }
             ptr = nextArg(ptr);
@@ -1279,7 +1289,7 @@ static void cliTempSensor(char *cmdline)
 static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint)
 {
     cliPrintLinef("#wp %d %svalid", posControl.waypointCount, posControl.waypointListValid ? "" : "in"); //int8_t bool
-    const char *format = "wp %u %u %d %d %d %d %u"; //uint8_t action; int32_t lat; int32_t lon; int32_t alt; int16_t p1; uint8_t flag
+    const char *format = "wp %u %u %d %d %d %d %d %d %u"; //uint8_t action; int32_t lat; int32_t lon; int32_t alt; int16_t p1 int16_t p2 int16_t p3; uint8_t flag
     for (uint8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) {
         bool equalsDefault = false;
         if (defaultNavWaypoint) {
@@ -1288,6 +1298,8 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
                 && navWaypoint[i].lon == defaultNavWaypoint[i].lon
                 && navWaypoint[i].alt == defaultNavWaypoint[i].alt
                 && navWaypoint[i].p1 == defaultNavWaypoint[i].p1
+                && navWaypoint[i].p2 == defaultNavWaypoint[i].p2
+                && navWaypoint[i].p3 == defaultNavWaypoint[i].p3
                 && navWaypoint[i].flag == defaultNavWaypoint[i].flag;
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
@@ -1296,6 +1308,8 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
                 defaultNavWaypoint[i].lon,
                 defaultNavWaypoint[i].alt,
                 defaultNavWaypoint[i].p1,
+                defaultNavWaypoint[i].p2,
+                defaultNavWaypoint[i].p3,
                 defaultNavWaypoint[i].flag
             );
         }
@@ -1306,6 +1320,8 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
             navWaypoint[i].lon,
             navWaypoint[i].alt,
             navWaypoint[i].p1,
+            navWaypoint[i].p2,
+            navWaypoint[i].p3,
             navWaypoint[i].flag
         );
     }
@@ -1322,7 +1338,7 @@ static void cliWaypoints(char *cmdline)
     } else if (sl_strcasecmp(cmdline, "save") == 0) {
         posControl.waypointListValid = false;
         for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
-            if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH)) break;
+            if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[i].action == NAV_WP_ACTION_LAND)) break;
             if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
                 posControl.waypointCount = i + 1;
                 posControl.waypointListValid = true;
@@ -1335,7 +1351,7 @@ static void cliWaypoints(char *cmdline)
             cliShowParseError();
         }
     } else {
-        int16_t i, p1;
+        int16_t i, p1,p2=0,p3=0,tmp;
         uint8_t action, flag;
         int32_t lat, lon, alt;
         uint8_t validArgumentCount = 0;
@@ -1369,12 +1385,29 @@ static void cliWaypoints(char *cmdline)
             }
             ptr = nextArg(ptr);
             if (ptr) {
-                flag = fastA2I(ptr);
+                tmp = fastA2I(ptr);
                 validArgumentCount++;
             }
-            if (validArgumentCount < 4) {
+                /* We support pre-2.5 6 values (... p1,flags) or
+                 *  2.5 and later, 8 values (... p1,p2,p3,flags)
+                 */
+            ptr = nextArg(ptr);
+            if (ptr) {
+                p2 = tmp;
+                p3 = fastA2I(ptr);
+                validArgumentCount++;
+                ptr = nextArg(ptr);
+                 if (ptr) {
+                    flag = fastA2I(ptr);
+                    validArgumentCount++;
+                }
+            } else {
+                flag = tmp;
+            }
+
+            if (!(validArgumentCount == 6 || validArgumentCount == 8)) {
                 cliShowParseError();
-            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH) || (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) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
                 cliShowParseError();
             } else {
                 posControl.waypointList[i].action = action;
@@ -1382,6 +1415,8 @@ static void cliWaypoints(char *cmdline)
                 posControl.waypointList[i].lon = lon;
                 posControl.waypointList[i].alt = alt;
                 posControl.waypointList[i].p1 = p1;
+                posControl.waypointList[i].p2 = p2;
+                posControl.waypointList[i].p3 = p3;
                 posControl.waypointList[i].flag = flag;
             }
         } else {
@@ -1732,7 +1767,7 @@ static void cliServoMix(char *cmdline)
 
 static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions)
 {
-    const char *format = "logic %d %d %d %d %d %d %d %d";
+    const char *format = "logic %d %d %d %d %d %d %d %d %d";
     for (uint32_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
         const logicCondition_t logic = logicConditions[i];
 
@@ -1741,6 +1776,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions
             logicCondition_t defaultValue = defaultLogicConditions[i];
             equalsDefault =
                 logic.enabled == defaultValue.enabled &&
+                logic.activatorId == defaultValue.activatorId &&
                 logic.operation == defaultValue.operation &&
                 logic.operandA.type == defaultValue.operandA.type &&
                 logic.operandA.value == defaultValue.operandA.value &&
@@ -1751,6 +1787,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
                 logic.enabled,
+                logic.activatorId,
                 logic.operation,
                 logic.operandA.type,
                 logic.operandA.value,
@@ -1762,6 +1799,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions
         cliDumpPrintLinef(dumpMask, equalsDefault, format,
             i,
             logic.enabled,
+            logic.activatorId,
             logic.operation,
             logic.operandA.type,
             logic.operandA.value,
@@ -1774,7 +1812,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions
 
 static void cliLogic(char *cmdline) {
     char * saveptr;
-    int args[8], check = 0;
+    int args[9], check = 0;
     uint8_t len = strlen(cmdline);
 
     if (len == 0) {
@@ -1785,6 +1823,7 @@ static void cliLogic(char *cmdline) {
         enum {
             INDEX = 0,
             ENABLED,
+            ACTIVATOR_ID,
             OPERATION,
             OPERAND_A_TYPE,
             OPERAND_A_VALUE,
@@ -1808,6 +1847,7 @@ static void cliLogic(char *cmdline) {
         if (
             i >= 0 && i < MAX_LOGIC_CONDITIONS &&
             args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
+            args[ACTIVATOR_ID] >= -1 && args[ACTIVATOR_ID] < MAX_LOGIC_CONDITIONS &&
             args[OPERATION] >= 0 && args[OPERATION] < LOGIC_CONDITION_LAST &&
             args[OPERAND_A_TYPE] >= 0 && args[OPERAND_A_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
             args[OPERAND_A_VALUE] >= -1000000 && args[OPERAND_A_VALUE] <= 1000000 &&
@@ -1817,6 +1857,7 @@ static void cliLogic(char *cmdline) {
 
         ) {
             logicConditionsMutable(i)->enabled = args[ENABLED];
+            logicConditionsMutable(i)->activatorId = args[ACTIVATOR_ID];
             logicConditionsMutable(i)->operation = args[OPERATION];
             logicConditionsMutable(i)->operandA.type = args[OPERAND_A_TYPE];
             logicConditionsMutable(i)->operandA.value = args[OPERAND_A_VALUE];
@@ -1830,6 +1871,83 @@ static void cliLogic(char *cmdline) {
         }
     }
 }
+
+static void printGvar(uint8_t dumpMask, const globalVariableConfig_t *gvars, const globalVariableConfig_t *defaultGvars)
+{
+    const char *format = "gvar %d %d %d %d";
+    for (uint32_t i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
+        const globalVariableConfig_t gvar = gvars[i];
+
+        bool equalsDefault = false;
+        if (defaultGvars) {
+            globalVariableConfig_t defaultValue = defaultGvars[i];
+            equalsDefault =
+                gvar.defaultValue == defaultValue.defaultValue &&
+                gvar.min == defaultValue.min &&
+                gvar.max == defaultValue.max;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format,
+                i,
+                gvar.defaultValue,
+                gvar.min,
+                gvar.max
+            );
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format,
+            i,
+            gvar.defaultValue,
+            gvar.min,
+            gvar.max
+        );
+    }
+}
+
+static void cliGvar(char *cmdline) {
+    char * saveptr;
+    int args[4], check = 0;
+    uint8_t len = strlen(cmdline);
+
+    if (len == 0) {
+        printGvar(DUMP_MASTER, globalVariableConfigs(0), NULL);
+    } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
+        pgResetCopy(globalVariableConfigsMutable(0), PG_GLOBAL_VARIABLE_CONFIG);
+    } else {
+        enum {
+            INDEX = 0,
+            DEFAULT,
+            MIN,
+            MAX,
+            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_GLOBAL_VARIABLES &&
+            args[DEFAULT] >= INT32_MIN && args[DEFAULT] <= INT32_MAX &&  
+            args[MIN] >= INT32_MIN && args[MIN] <= INT32_MAX &&  
+            args[MAX] >= INT32_MIN && args[MAX] <= INT32_MAX  
+        ) {
+            globalVariableConfigsMutable(i)->defaultValue = args[DEFAULT];
+            globalVariableConfigsMutable(i)->min = args[MIN];
+            globalVariableConfigsMutable(i)->max = args[MAX];
+
+            cliGvar("");
+        } else {
+            cliShowParseError();
+        }
+    }
+}
+
 #endif
 
 #ifdef USE_GLOBAL_FUNCTIONS
@@ -2006,12 +2124,32 @@ static void cliSdInfo(char *cmdline)
 
 static void cliFlashInfo(char *cmdline)
 {
-    const flashGeometry_t *layout = flashfsGetGeometry();
-
     UNUSED(cmdline);
 
-    cliPrintLinef("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u, usedSize=%u",
-            layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize, flashfsGetOffset());
+    const flashGeometry_t *layout = flashGetGeometry();
+
+    cliPrintLinef("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u",
+            layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize);
+
+    for (uint8_t index = 0; index < FLASH_MAX_PARTITIONS; index++) {
+        const flashPartition_t *partition;
+        if (index == 0) {
+            cliPrintLine("Paritions:");
+        }
+        partition = flashPartitionFindByIndex(index);
+        if (!partition) {
+            break;
+        }
+        cliPrintLinef("  %d: %s %u %u", index, flashPartitionGetTypeName(partition->type), partition->startSector, partition->endSector);
+    }
+#ifdef USE_FLASHFS
+    const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FLASHFS);
+
+    cliPrintLinef("FlashFS size=%u, usedSize=%u",
+            FLASH_PARTITION_SECTOR_COUNT(flashPartition) * layout->sectorSize,
+            flashfsGetOffset()
+    );
+#endif
 }
 
 static void cliFlashErase(char *cmdline)
@@ -2021,7 +2159,7 @@ static void cliFlashErase(char *cmdline)
     cliPrintLine("Erasing...");
     flashfsEraseCompletely();
 
-    while (!flashfsIsReady()) {
+    while (!flashIsReady()) {
         delay(100);
     }
 
@@ -3186,6 +3324,9 @@ static void printConfig(const char *cmdline, bool doDiff)
 #ifdef USE_LOGIC_CONDITIONS
         cliPrintHashLine("logic");
         printLogic(dumpMask, logicConditions_CopyArray, logicConditions(0));
+
+        cliPrintHashLine("gvar");
+        printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
 #endif
 
 #ifdef USE_GLOBAL_FUNCTIONS
@@ -3322,11 +3463,7 @@ static void cliMsc(char *cmdline)
         waitForSerialPortToFinishTransmitting(cliPort);
         stopPwmAllMotors();
 
-#ifdef STM32F7
-        *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC;
-#elif defined(STM32F4)
-        *((uint32_t *)0x2001FFF0) = MSC_MAGIC;
-#endif
+        persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_MSC_REQUEST);
 
         __disable_irq();
         NVIC_SystemReset();
@@ -3441,8 +3578,12 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
 #ifdef USE_LOGIC_CONDITIONS
     CLI_COMMAND_DEF("logic", "configure logic conditions",
-        "<rule> <enabled> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>\r\n"
+        "<rule> <enabled> <activatorId> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>\r\n"
         "\treset\r\n", cliLogic),
+
+    CLI_COMMAND_DEF("gvar", "configure global variables",
+        "<gvar> <default> <min> <max>\r\n"
+        "\treset\r\n", cliGvar),
 #endif
 #ifdef USE_GLOBAL_FUNCTIONS
     CLI_COMMAND_DEF("gf", "configure global functions",
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 4ac930f1fa..20b08e5ca2 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -217,7 +217,15 @@ void validateAndFixConfig(void)
 #endif
 
 #ifndef USE_PWM_SERVO_DRIVER
-    featureClear(FEATURE_PWM_SERVO_DRIVER);
+    if (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) {
+        servoConfigMutable()->servo_protocol = SERVO_TYPE_PWM;
+    }
+#endif
+
+#ifndef USE_SERVO_SBUS
+    if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) {
+        servoConfigMutable()->servo_protocol = SERVO_TYPE_PWM;
+    }
 #endif
 
     if (!isSerialConfigValid(serialConfigMutable())) {
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index a12fc63a07..bab2d96eef 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -62,8 +62,8 @@ typedef enum {
     FEATURE_SUPEREXPO_RATES = 1 << 23,
     FEATURE_VTX = 1 << 24,
     FEATURE_UNUSED_8 = 1 << 25,         // RX_SPI
-    FEATURE_UNUSED_9 = 1 << 26,         //SOFTSPI
-    FEATURE_PWM_SERVO_DRIVER = 1 << 27,
+    FEATURE_UNUSED_9 = 1 << 26,         // SOFTSPI
+    FEATURE_UNUSED_11 = 1 << 27,        // FEATURE_PWM_SERVO_DRIVER
     FEATURE_PWM_OUTPUT_ENABLE = 1 << 28,
     FEATURE_OSD = 1 << 29,
     FEATURE_FW_LAUNCH = 1 << 30,
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 11ec133532..dc50817963 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "blackbox/blackbox.h"
 
 #include "build/debug.h"
@@ -48,6 +50,7 @@
 #include "sensors/battery.h"
 #include "sensors/rangefinder.h"
 #include "sensors/opflow.h"
+#include "sensors/esc_sensor.h"
 
 #include "fc/fc_core.h"
 #include "fc/cli.h"
@@ -200,7 +203,7 @@ static void updateArmingStatus(void)
         /* CHECK: Throttle */
         if (!armingConfig()->fixed_wing_auto_arm) {
             // Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
-            if (calculateThrottleStatus() != THROTTLE_LOW) {
+            if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
                 ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
             } else {
                 DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
@@ -475,6 +478,8 @@ void tryArm(void)
 
         ENABLE_ARMING_FLAG(ARMED);
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
+        //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();
         headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
@@ -529,7 +534,7 @@ void processRx(timeUs_t currentTimeUs)
 
     failsafeUpdateState();
 
-    const throttleStatus_e throttleStatus = calculateThrottleStatus();
+    const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
 
     // When armed and motors aren't spinning, do beeps periodically
     if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
@@ -852,6 +857,10 @@ void taskRunRealtimeCallbacks(timeUs_t currentTimeUs)
 #ifdef USE_DSHOT
     pwmCompleteMotorUpdate();
 #endif
+
+#ifdef USE_ESC_SENSOR
+    escSensorUpdate(currentTimeUs);
+#endif
 }
 
 bool taskUpdateRxCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 990c02aa3d..6f7f0b9b6e 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -35,6 +35,7 @@
 #include "common/maths.h"
 #include "common/memory.h"
 #include "common/printf.h"
+#include "common/global_variables.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -53,9 +54,11 @@
 #include "drivers/flash_m25p16.h"
 #include "drivers/io.h"
 #include "drivers/io_pca9685.h"
+#include "drivers/flash.h"
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
 #include "drivers/osd.h"
+#include "drivers/persistent.h"
 #include "drivers/pwm_esc_detect.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/pwm_output.h"
@@ -135,6 +138,7 @@
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
+#include "sensors/esc_sensor.h"
 
 #include "scheduler/scheduler.h"
 
@@ -182,6 +186,10 @@ void flashLedsAndBeep(void)
 
 void init(void)
 {
+#if defined(USE_FLASHFS) && defined(USE_FLASH_M25P16)
+    bool flashDeviceInitialized = false;
+#endif
+
 #ifdef USE_HAL_DRIVER
     HAL_Init();
 #endif
@@ -210,7 +218,9 @@ void init(void)
     // Re-initialize system clock to their final values (if necessary)
     systemClockSetup(systemConfig()->cpuUnderclock);
 
+#ifdef USE_I2C
     i2cSetSpeed(systemConfig()->i2c_speed);
+#endif
 
 #ifdef USE_HARDWARE_PREBOOT_SETUP
     initialisePreBootHardware();
@@ -265,6 +275,12 @@ void init(void)
     // to run after the sensors have been detected.
     mspSerialInit();
 
+#ifdef USE_ESC_SENSOR
+    // DSHOT supports a dedicated wire ESC telemetry. Kick off the ESC-sensor receiver initialization
+    // We may, however, do listen_only, so need to init this anyway
+    escSensorInitialize();
+#endif
+
 #if defined(USE_DJI_HD_OSD)
     // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task
     djiOsdSerialInit();
@@ -276,6 +292,10 @@ void init(void)
     logInit();
 #endif
 
+#ifdef USE_LOGIC_CONDITIONS
+    gvInit();
+#endif
+
     // Initialize servo and motor mixers
     // This needs to be called early to set up platform type correctly and count required motors & servos
     servosInit();
@@ -355,7 +375,10 @@ void init(void)
         if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
 #ifdef USE_FLASH_M25P16
             // Must initialise the device to read _anything_
-            m25p16_init(0);
+            /*m25p16_init(0);*/
+            if (!flashDeviceInitialized) {
+                flashDeviceInitialized = flashInit();
+            }
 #endif
             emfat_init_files();
         }
@@ -509,7 +532,7 @@ void init(void)
 
     rxInit();
 
-#if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)))
+#if defined(USE_OSD)
     displayPort_t *osdDisplayPort = NULL;
 #endif
 
@@ -536,13 +559,6 @@ void init(void)
     }
 #endif
 
-#if defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)
-    // If OSD is not active, then register MSP_DISPLAYPORT as a CMS device.
-    if (!osdDisplayPort) {
-        cmsDisplayPortRegister(displayPortMspInit());
-    }
-#endif
-
 #ifdef USE_UAV_INTERCONNECT
     uavInterconnectBusInit();
 #endif
@@ -579,7 +595,9 @@ void init(void)
 #ifdef USE_FLASHFS
         case BLACKBOX_DEVICE_FLASH:
 #ifdef USE_FLASH_M25P16
-            m25p16_init(0);
+            if (!flashDeviceInitialized) {
+                flashDeviceInitialized = flashInit();
+            }
 #endif
             flashfsInit();
             break;
@@ -632,12 +650,6 @@ void init(void)
     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
         batteryInit();
 
-#ifdef USE_PWM_SERVO_DRIVER
-    if (feature(FEATURE_PWM_SERVO_DRIVER)) {
-        pwmDriverInitialize();
-    }
-#endif
-
 #ifdef USE_RCDEVICE
     rcdeviceInit();
 #endif // USE_RCDEVICE
@@ -661,5 +673,8 @@ void init(void)
     }
 #endif
 
+    // Considering that the persistent reset reason is only used during init
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
     systemState |= SYSTEM_STATE_READY;
 }
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 212b7f01f3..36427cc095 100644
--- 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 "common/global_functions.h"
+#include "common/global_variables.h"
 
 #include "config/parameter_group_ids.h"
 
@@ -43,6 +44,7 @@
 #include "drivers/bus_i2c.h"
 #include "drivers/compass/compass.h"
 #include "drivers/display.h"
+#include "drivers/flash.h"
 #include "drivers/osd.h"
 #include "drivers/osd_symbols.h"
 #include "drivers/pwm_mapping.h"
@@ -292,8 +294,8 @@ static void serializeSDCardSummaryReply(sbuf_t *dst)
 static void serializeDataflashSummaryReply(sbuf_t *dst)
 {
 #ifdef USE_FLASHFS
-    const flashGeometry_t *geometry = flashfsGetGeometry();
-    sbufWriteU8(dst, flashfsIsReady() ? 1 : 0);
+    const flashGeometry_t *geometry = flashGetGeometry();
+    sbufWriteU8(dst, flashIsReady() ? 1 : 0);
     sbufWriteU32(dst, geometry->sectors);
     sbufWriteU32(dst, geometry->totalSize);
     sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
@@ -529,6 +531,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     case MSP2_INAV_LOGIC_CONDITIONS:
         for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
             sbufWriteU8(dst, logicConditions(i)->enabled);
+            sbufWriteU8(dst, logicConditions(i)->activatorId);
             sbufWriteU8(dst, logicConditions(i)->operation);
             sbufWriteU8(dst, logicConditions(i)->operandA.type);
             sbufWriteU32(dst, logicConditions(i)->operandA.value);
@@ -542,6 +545,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU32(dst, logicConditionGetValue(i));
         }
         break;
+    case MSP2_INAV_GVAR_STATUS:
+        for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
+            sbufWriteU32(dst, gvGet(i));
+        }
+        break;
 #endif
 #ifdef USE_GLOBAL_FUNCTIONS
     case MSP2_INAV_GLOBAL_FUNCTIONS:
@@ -1265,7 +1273,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
 
     case MSP_CALIBRATION_DATA:
-    #ifdef USE_ACC
         sbufWriteU8(dst, accGetCalibrationAxisFlags());
         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[X]);
         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Y]);
@@ -1273,15 +1280,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[X]);
         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Y]);
         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Z]);
-    #else
-        sbufWriteU8(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-    #endif
 
     #ifdef USE_MAG
         sbufWriteU16(dst, compassConfig()->magZero.raw[X]);
@@ -1943,8 +1941,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #ifdef USE_LOGIC_CONDITIONS
     case MSP2_INAV_SET_LOGIC_CONDITIONS:
         sbufReadU8Safe(&tmp_u8, src);
-        if ((dataSize == 14) && (tmp_u8 < MAX_LOGIC_CONDITIONS)) {
+        if ((dataSize == 15) && (tmp_u8 < MAX_LOGIC_CONDITIONS)) {
             logicConditionsMutable(tmp_u8)->enabled = sbufReadU8(src);
+            logicConditionsMutable(tmp_u8)->activatorId = sbufReadU8(src);
             logicConditionsMutable(tmp_u8)->operation = sbufReadU8(src);
             logicConditionsMutable(tmp_u8)->operandA.type = sbufReadU8(src);
             logicConditionsMutable(tmp_u8)->operandA.value = sbufReadU32(src);
@@ -1958,7 +1957,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #ifdef USE_GLOBAL_FUNCTIONS
     case MSP2_INAV_SET_GLOBAL_FUNCTIONS:
         sbufReadU8Safe(&tmp_u8, src);
-        if ((dataSize == 14) && (tmp_u8 < MAX_GLOBAL_FUNCTIONS)) {
+        if ((dataSize == 10) && (tmp_u8 < MAX_GLOBAL_FUNCTIONS)) {
             globalFunctionsMutable(tmp_u8)->enabled = sbufReadU8(src);
             globalFunctionsMutable(tmp_u8)->conditionId = sbufReadU8(src);
             globalFunctionsMutable(tmp_u8)->action = sbufReadU8(src);
@@ -2198,21 +2197,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_SET_CALIBRATION_DATA:
         if (dataSize >= 18) {
-#ifdef USE_ACC
             accelerometerConfigMutable()->accZero.raw[X] = sbufReadU16(src);
             accelerometerConfigMutable()->accZero.raw[Y] = sbufReadU16(src);
             accelerometerConfigMutable()->accZero.raw[Z] = sbufReadU16(src);
             accelerometerConfigMutable()->accGain.raw[X] = sbufReadU16(src);
             accelerometerConfigMutable()->accGain.raw[Y] = sbufReadU16(src);
             accelerometerConfigMutable()->accGain.raw[Z] = sbufReadU16(src);
-#else
-            sbufReadU16(src);
-            sbufReadU16(src);
-            sbufReadU16(src);
-            sbufReadU16(src);
-            sbufReadU16(src);
-            sbufReadU16(src);
-#endif
 
 #ifdef USE_MAG
             compassConfigMutable()->magZero.raw[X] = sbufReadU16(src);
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index e8ba6f432a..7a5c64b06d 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -34,6 +34,7 @@
 #include "drivers/sensor.h"
 #include "drivers/serial.h"
 #include "drivers/stack_check.h"
+#include "drivers/pwm_mapping.h"
 
 #include "fc/cli.h"
 #include "fc/config.h"
@@ -48,6 +49,7 @@
 #include "flight/pid.h"
 #include "flight/wind_estimator.h"
 #include "flight/rpm_filter.h"
+#include "flight/servos.h"
 
 #include "navigation/navigation.h"
 
@@ -62,6 +64,7 @@
 #include "io/rcdevice_cam.h"
 #include "io/vtx.h"
 #include "io/osd_dji_hd.h"
+#include "io/servo_sbus.h"
 
 #include "msp/msp_serial.h"
 
@@ -78,6 +81,7 @@
 #include "sensors/battery.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
+#include "sensors/irlock.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 #include "sensors/opflow.h"
@@ -209,6 +213,14 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 }
 #endif
 
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+void taskUpdateIrlock(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    irlockUpdate();
+}
+#endif
+
 #ifdef USE_OPFLOW
 void taskUpdateOpticalFlow(timeUs_t currentTimeUs)
 {
@@ -249,17 +261,19 @@ void taskLedStrip(timeUs_t currentTimeUs)
 }
 #endif
 
-#ifdef USE_PWM_SERVO_DRIVER
-void taskSyncPwmDriver(timeUs_t currentTimeUs)
+void taskSyncServoDriver(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
 
-    if (feature(FEATURE_PWM_SERVO_DRIVER)) {
-        pwmDriverSync();
-    }
-}
+#if defined(USE_SERVO_SBUS)
+    sbusServoSendUpdate();
 #endif
 
+#ifdef USE_PWM_SERVO_DRIVER
+    pwmDriverSync();
+#endif
+}
+
 #ifdef USE_OSD
 void taskUpdateOsd(timeUs_t currentTimeUs)
 {
@@ -317,8 +331,8 @@ void fcTasksInit(void)
 #ifdef STACK_CHECK
     setTaskEnabled(TASK_STACK_CHECK, true);
 #endif
-#ifdef USE_PWM_SERVO_DRIVER
-    setTaskEnabled(TASK_PWMDRIVER, feature(FEATURE_PWM_SERVO_DRIVER));
+#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));
 #endif
 #ifdef USE_CMS
 #ifdef USE_MSP_DISPLAYPORT
@@ -347,6 +361,9 @@ void fcTasksInit(void)
 #ifdef USE_GLOBAL_FUNCTIONS
     setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true);
 #endif
+#ifdef USE_IRLOCK
+    setTaskEnabled(TASK_IRLOCK, irlockHasBeenDetected());
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -454,6 +471,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_IRLOCK
+    [TASK_IRLOCK] = {
+        .taskName = "IRLOCK",
+        .taskFunc = taskUpdateIrlock,
+        .desiredPeriod = TASK_PERIOD_HZ(100),
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
 #ifdef USE_DASHBOARD
     [TASK_DASHBOARD] = {
         .taskName = "DASHBOARD",
@@ -481,10 +507,10 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#ifdef USE_PWM_SERVO_DRIVER
+#if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS)
     [TASK_PWMDRIVER] = {
-        .taskName = "PWMDRIVER",
-        .taskFunc = taskSyncPwmDriver,
+        .taskName = "SERVOS",
+        .taskFunc = taskSyncServoDriver,
         .desiredPeriod = TASK_PERIOD_HZ(200),         // 200 Hz
         .staticPriority = TASK_PRIORITY_HIGH,
     },
diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index b741e75b87..1b8664972d 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -74,7 +74,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONT
 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
     .deadband = 5,
     .yaw_deadband = 5,
-    .pos_hold_deadband = 20,
+    .pos_hold_deadband = 10,
     .alt_hold_deadband = 50,
     .mid_throttle_deadband = 50,
     .airmodeHandlingType = STICK_CENTER,
@@ -99,12 +99,19 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
     return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
 }
 
-throttleStatus_e calculateThrottleStatus(void)
+throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
 {
+    int value;
+    if (type == THROTTLE_STATUS_TYPE_RC) {
+        value = rxGetChannelValue(THROTTLE);
+    } else {
+        value = rcCommand[THROTTLE];
+    }
+
     const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
-    if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
+    if (feature(FEATURE_REVERSIBLE_MOTORS) && (value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
         return THROTTLE_LOW;
-    else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
+    else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))
         return THROTTLE_LOW;
 
     return THROTTLE_HIGH;
diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h
index 23c8887cfa..b985c60394 100644
--- a/src/main/fc/rc_controls.h
+++ b/src/main/fc/rc_controls.h
@@ -45,6 +45,11 @@ typedef enum {
     THROTTLE_HIGH
 } throttleStatus_e;
 
+typedef enum {
+    THROTTLE_STATUS_TYPE_RC = 0,
+    THROTTLE_STATUS_TYPE_COMMAND
+} throttleStatusType_e;
+
 typedef enum {
     NOT_CENTERED = 0,
     CENTERED
@@ -100,7 +105,7 @@ bool checkStickPosition(stickPositions_e stickPos);
 
 bool areSticksInApModePosition(uint16_t ap_mode);
 bool areSticksDeflectedMoreThanPosHoldDeadband(void);
-throttleStatus_e calculateThrottleStatus(void);
+throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
 rollPitchStatus_e calculateRollPitchCenterStatus(void);
 void processRcStickPositions(throttleStatus_e throttleStatus);
 
diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c
index 0d1a60a25c..cb7af593bb 100644
--- a/src/main/fc/runtime_config.c
+++ b/src/main/fc/runtime_config.c
@@ -20,6 +20,7 @@
 
 #include "platform.h"
 
+#include "common/utils.h"
 #include "fc/runtime_config.h"
 
 #include "io/beeper.h"
@@ -39,16 +40,51 @@ const char *armingDisableFlagNames[]= {
 };
 #endif
 
+const armingFlag_e armDisableReasonsChecklist[] = {
+    ARMING_DISABLED_INVALID_SETTING,
+    ARMING_DISABLED_HARDWARE_FAILURE,
+    ARMING_DISABLED_PWM_OUTPUT_ERROR,
+    ARMING_DISABLED_COMPASS_NOT_CALIBRATED,
+    ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED,
+    ARMING_DISABLED_RC_LINK,
+    ARMING_DISABLED_NAVIGATION_UNSAFE,
+    ARMING_DISABLED_ARM_SWITCH,
+    ARMING_DISABLED_BOXFAILSAFE,
+    ARMING_DISABLED_BOXKILLSWITCH,
+    ARMING_DISABLED_THROTTLE,
+    ARMING_DISABLED_CLI,
+    ARMING_DISABLED_CMS_MENU,
+    ARMING_DISABLED_OSD_MENU,
+    ARMING_DISABLED_ROLLPITCH_NOT_CENTERED,
+    ARMING_DISABLED_SERVO_AUTOTRIM,
+    ARMING_DISABLED_OOM
+};
+
 armingFlag_e isArmingDisabledReason(void)
 {
-    armingFlag_e flag;
+    // Shortcut, if we don't block arming at all
+    if (!isArmingDisabled()) {
+        return 0;
+    }
+
     armingFlag_e reasons = armingFlags & ARMING_DISABLED_ALL_FLAGS;
-    for (unsigned ii = 0; ii < sizeof(armingFlag_e) * 8; ii++) {
-        flag = 1u << ii;
+
+    // First check for "more important reasons"
+    for (unsigned ii = 0; ii < ARRAYLEN(armDisableReasonsChecklist); ii++) {
+        armingFlag_e flag = armDisableReasonsChecklist[ii];
         if (flag & reasons) {
             return flag;
         }
     }
+
+    // Fallback, we ended up with a blocker flag not included in armDisableReasonsChecklist[]
+    for (unsigned ii = 0; ii < sizeof(armingFlag_e) * 8; ii++) {
+        armingFlag_e flag = 1u << ii;
+        if (flag & reasons) {
+            return flag;
+        }
+    }
+
     return 0;
 }
 
@@ -80,7 +116,7 @@ uint32_t disableFlightMode(flightModeFlags_e mask)
     return flightModeFlags;
 }
 
-bool FAST_CODE NOINLINE sensors(uint32_t mask)
+bool sensors(uint32_t mask)
 {
     return enabledSensors & mask;
 }
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index b44b1ae899..d24ea66a25 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -129,7 +129,8 @@ typedef enum {
     BOAT                                = (1 << 20),
     ALTITUDE_CONTROL                    = (1 << 21),    //It means it can fly
     MOVE_FORWARD_ONLY                   = (1 << 22),
-    FW_HEADING_USE_YAW                  = (1 << 23),
+    SET_REVERSIBLE_MOTORS_FORWARD       = (1 << 23),
+    FW_HEADING_USE_YAW                  = (1 << 24),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index e59925478d..5dd2b4bb7e 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -33,6 +33,8 @@ tables:
     values: ["SERIAL", "SPIFLASH", "SDCARD"]
   - name: motor_pwm_protocol
     values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"]
+  - name: servo_protocol
+    values: ["PWM", "SERVO_DRIVER", "SBUS"]
   - name: failsafe_procedure
     values: ["SET-THR", "DROP", "RTH", "NONE"]
   - name: current_sensor
@@ -81,7 +83,8 @@ tables:
     values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
-      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY"]
+      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
+      "IRLOCK"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -138,6 +141,9 @@ tables:
   - name: pidTypeTable
     values: ["NONE", "PID", "PIFF", "AUTO"]
     enum: pidType_e
+  - name: osd_ahi_style
+    values: ["DEFAULT", "LINE"]
+    enum: osd_ahi_style_e
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -202,7 +208,7 @@ groups:
       - name: dynamic_gyro_notch_min_hz
         field: dynamicGyroNotchMinHz
         condition: USE_DYNAMIC_FILTERS
-        min: 60
+        min: 30
         max: 1000
       - name: gyro_to_use
         condition: USE_DUAL_GYRO
@@ -723,6 +729,9 @@ groups:
     type: servoConfig_t
     headers: ["flight/servos.h"]
     members:
+      - name: servo_protocol
+        field: servo_protocol
+        table: servo_protocol
       - name: servo_center_pulse
         field: servoCenterPulse
         min: PWM_RANGE_MIN
@@ -885,7 +894,7 @@ groups:
       - name: rpm_gyro_min_hz
         field: gyro_min_hz
         type: uint8_t
-        min: 50
+        min: 30
         max: 200
       - name: rpm_gyro_q
         field: gyro_q
@@ -933,7 +942,7 @@ groups:
         min: 0
         max: 100
       - name: pos_hold_deadband
-        min: 10
+        min: 2
         max: 250
       - name: alt_hold_deadband
         min: 10
@@ -1247,7 +1256,7 @@ groups:
         min: 0
         max: 255
       - name: nav_fw_pos_hdg_pidsum_limit
-        field: pidSumLimitYaw
+        field: navFwPosHdgPidsumLimit
         min: PID_SUM_LIMIT_MIN
         max: PID_SUM_LIMIT_MAX
       - name: mc_iterm_relax_type
@@ -1605,6 +1614,10 @@ groups:
         field: fw.cruise_speed
         min: 0
         max: 65535
+      - name: nav_fw_control_smoothness 
+        field: fw.control_smoothness
+        min: 0
+        max: 9
 
       - name: nav_fw_land_dive_angle
         field: fw.land_dive_angle
@@ -1901,6 +1914,14 @@ groups:
         field: imu_temp_alarm_max
         min: -550
         max: 1250
+      - name: osd_esc_temp_alarm_max
+        field: esc_temp_alarm_max
+        min: -550
+        max: 1500
+      - name: osd_esc_temp_alarm_min
+        field: esc_temp_alarm_min
+        min: -550
+        max: 1500
       - name: osd_baro_temp_alarm_min
         field: baro_temp_alarm_min
         condition: USE_BARO
@@ -1950,8 +1971,8 @@ groups:
         max: 4
       - name: osd_hud_margin_v
         field: hud_margin_v
-        min: 0
-        max: 4
+        min: 1
+        max: 3
       - name: osd_hud_homing
         field: hud_homing
         type: bool
@@ -1974,6 +1995,10 @@ groups:
         field: hud_radar_nearest
         min: 0
         max: 4000
+      - name: osd_hud_wp_disp
+        field: hud_wp_disp
+        min: 0
+        max: 3
       - name: osd_left_sidebar_scroll
         field: left_sidebar_scroll
         table: osd_sidebar_scroll
@@ -2007,6 +2032,9 @@ groups:
         min: 10
         max: 13
 
+      - name: osd_ahi_style
+        table: osd_ahi_style
+
   - name: PG_SYSTEM_CONFIG
     type: systemConfig_t
     headers: ["fc/config.h"]
@@ -2140,5 +2168,14 @@ groups:
           table: log_level
         - name: log_topics
           field: topics
-          min: 0,
+          min: 0
           max: UINT32_MAX
+
+  - name: PG_ESC_SENSOR_CONFIG
+    type: escSensorConfig_t
+    headers: ["sensors/esc_sensor.h"]
+    condition: USE_ESC_SENSOR
+    members:
+        - name: esc_sensor_listen_only
+          field: listenOnly
+          type: bool
diff --git a/src/main/flight/dynamic_gyro_notch.c b/src/main/flight/dynamic_gyro_notch.c
index f45b3b6c09..7e6c12afbc 100644
--- a/src/main/flight/dynamic_gyro_notch.c
+++ b/src/main/flight/dynamic_gyro_notch.c
@@ -40,15 +40,14 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
     state->looptime = getLooptime();
 
     if (state->enabled) {
-        const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
-
         /*
          * Step 1 - init all filters even if they will not be used further down the road
          */
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
-            biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
-            biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
+            //Any initial notch Q is valid sice it will be updated immediately after
+            biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+            biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+            biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
         }
 
         state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h
index f6ae3b7779..87d923fb06 100644
--- a/src/main/flight/dynamic_gyro_notch.h
+++ b/src/main/flight/dynamic_gyro_notch.h
@@ -29,7 +29,6 @@
 #include "common/filter.h"
 
 #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
-#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
 
 typedef struct dynamicGyroNotchState_s {
     uint16_t frequency[XYZ_AXIS_COUNT];
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index c0f951dcc3..6ac2783c18 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -397,7 +397,7 @@ void failsafeUpdateState(void)
             case FAILSAFE_IDLE:
                 if (armed) {
                     // Track throttle command below minimum time
-                    if (THROTTLE_HIGH == calculateThrottleStatus()) {
+                    if (THROTTLE_HIGH == calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC)) {
                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
                     }
                     if (!receivingRxDataAndNotFailsafeMode) {
diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c
index 64da4305f1..b85d538a15 100644
--- a/src/main/flight/gyroanalyse.c
+++ b/src/main/flight/gyroanalyse.c
@@ -26,6 +26,7 @@
 #include <stdint.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #ifdef USE_DYNAMIC_FILTERS
 
diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h
index 0b4e060a4d..b6de10e40a 100644
--- a/src/main/flight/gyroanalyse.h
+++ b/src/main/flight/gyroanalyse.h
@@ -51,23 +51,6 @@ typedef struct gyroAnalyseState_s {
     biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
     uint16_t centerFreq[XYZ_AXIS_COUNT];
     uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
-
-    /*
-     * Extended Dynamic Filters are 3x3 filter matrix
-     * In this approach, we assume that vibration peak on one axis
-     * can be also detected on other axises, but with lower amplitude
-     * that causes this freqency not to be attenuated.
-     * 
-     * This approach is similiar to the approach on RPM filter when motor base
-     * frequency is attenuated on every axis even tho it might not be appearing
-     * in gyro traces
-     * 
-     * extendedDynamicFilter[GYRO_AXIS][ANALYZED_AXIS]
-     * 
-     */
-    biquadFilter_t extendedDynamicFilter[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
-    filterApplyFnPtr extendedDynamicFilterApplyFn;
-
     bool filterUpdateExecute;
     uint8_t filterUpdateAxis;
     uint16_t filterUpdateFrequency;
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index ff9f0f9063..375a128f89 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -23,6 +23,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "blackbox/blackbox.h"
 
 #include "build/build_config.h"
@@ -631,7 +633,7 @@ void imuCheckVibrationLevels(void)
     // DEBUG_VIBE values 4-7 are used by NAV estimator
 }
 
-void FAST_CODE NOINLINE imuUpdateAttitude(timeUs_t currentTimeUs)
+void imuUpdateAttitude(timeUs_t currentTimeUs)
 {
     /* Calculate dT */
     static timeUs_t previousIMUUpdateTimeUs;
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 4d8477cfe5..24aab18727 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "build/debug.h"
 
 #include "common/axis.h"
@@ -63,6 +65,11 @@ static EXTENDED_FASTRAM uint8_t motorCount = 0;
 EXTENDED_FASTRAM int mixerThrottleCommand;
 static EXTENDED_FASTRAM int throttleIdleValue = 0;
 static EXTENDED_FASTRAM int motorValueWhenStopped = 0;
+static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
+static EXTENDED_FASTRAM int throttleDeadbandLow = 0;
+static EXTENDED_FASTRAM int throttleDeadbandHigh = 0;
+static EXTENDED_FASTRAM int throttleRangeMin = 0;
+static EXTENDED_FASTRAM int throttleRangeMax = 0;
 static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
@@ -133,7 +140,7 @@ static void computeMotorCount(void)
     }
 }
 
-uint8_t FAST_CODE NOINLINE getMotorCount(void) {
+uint8_t getMotorCount(void) {
     return motorCount;
 }
 
@@ -239,6 +246,9 @@ void mixerInit(void)
         mixerScale = 0.5f;
     }
 
+    throttleDeadbandLow = PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband;
+    throttleDeadbandHigh = PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband;
+
     mixerResetDisarmedMotors();
 
     if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
@@ -256,7 +266,19 @@ void mixerInit(void)
 
 void mixerResetDisarmedMotors(void)
 {
-    const int motorZeroCommand = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
+    int motorZeroCommand;
+    
+    if (feature(FEATURE_REVERSIBLE_MOTORS)) {
+        motorZeroCommand = reversibleMotorsConfig()->neutral;
+        throttleRangeMin = throttleDeadbandHigh;
+        throttleRangeMax = motorConfig()->maxthrottle;
+    } else {
+        motorZeroCommand = motorConfig()->mincommand;
+        throttleRangeMin = getThrottleIdleValue();
+        throttleRangeMax = motorConfig()->maxthrottle;
+    }
+
+    reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
 
     if (feature(FEATURE_MOTOR_STOP)) {
         motorValueWhenStopped = motorZeroCommand;
@@ -270,7 +292,37 @@ void mixerResetDisarmedMotors(void)
     }
 }
 
-void FAST_CODE NOINLINE writeMotors(void)
+#ifdef USE_DSHOT
+static uint16_t handleOutputScaling(
+    int16_t input,          // Input value from the mixer
+    int16_t stopThreshold,  // Threshold value to check if motor should be rotating or not
+    int16_t onStopValue,    // Value sent to the ESC when min rotation is required - on motor_stop it is STOP command, without motor_stop it's a value that keeps rotation
+    int16_t inputScaleMin,  // Input range - min value
+    int16_t inputScaleMax,  // Input range - max value
+    int16_t outputScaleMin, // Output range - min value
+    int16_t outputScaleMax, // Output range - max value
+    bool moveForward        // If motor should be rotating FORWARD or BACKWARD
+)
+{
+    int value;
+    if (moveForward && input < stopThreshold) {
+        //Send motor stop command
+        value = onStopValue;
+    }
+    else if (!moveForward && input > stopThreshold) {
+        //Send motor stop command
+        value = onStopValue;
+    }
+    else {
+        //Scale input to protocol output values
+        value = scaleRangef(input, inputScaleMin, inputScaleMax, outputScaleMin, outputScaleMax);
+        value = constrain(value, outputScaleMin, outputScaleMax);
+    }
+    return value;
+}
+#endif
+
+void FAST_CODE writeMotors(void)
 {
     for (int i = 0; i < motorCount; i++) {
         uint16_t motorValue;
@@ -280,30 +332,72 @@ void FAST_CODE NOINLINE writeMotors(void)
         if (isMotorProtocolDigital()) {
 
             if (feature(FEATURE_REVERSIBLE_MOTORS)) {
-                if (motor[i] >= throttleIdleValue && motor[i] <= reversibleMotorsConfig()->deadband_low) {
-                    motorValue = scaleRangef(motor[i], motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE);
-                    motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW);
-                }
-                else if (motor[i] >= reversibleMotorsConfig()->deadband_high && motor[i] <= motorConfig()->maxthrottle) {
-                    motorValue = scaleRangef(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
-                    motorValue = constrain(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
-                }
-                else {
-                    motorValue = DSHOT_DISARM_COMMAND;
+                if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
+                    motorValue = handleOutputScaling(
+                        motor[i],
+                        throttleRangeMin,
+                        DSHOT_DISARM_COMMAND,
+                        throttleRangeMin,
+                        throttleRangeMax,
+                        DSHOT_3D_DEADBAND_HIGH,
+                        DSHOT_MAX_THROTTLE,
+                        true
+                    );
+                } else {
+                    motorValue = handleOutputScaling(
+                        motor[i],
+                        throttleRangeMax,
+                        DSHOT_DISARM_COMMAND,
+                        throttleRangeMin,
+                        throttleRangeMax,
+                        DSHOT_MIN_THROTTLE,
+                        DSHOT_3D_DEADBAND_LOW,
+                        false
+                    );
                 }
             }
             else {
-                if (motor[i] < throttleIdleValue) {    // motor disarmed
-                    motorValue = DSHOT_DISARM_COMMAND;
-                }
-                else {
-                    motorValue = scaleRangef(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
-                    motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
-                }
+                motorValue = handleOutputScaling(
+                    motor[i],
+                    throttleIdleValue,
+                    DSHOT_DISARM_COMMAND,
+                    motorConfig()->mincommand,
+                    motorConfig()->maxthrottle,
+                    DSHOT_MIN_THROTTLE,
+                    DSHOT_MAX_THROTTLE,
+                    true
+                );
             }
         }
         else {
-            motorValue = motor[i];
+            if (feature(FEATURE_REVERSIBLE_MOTORS)) {
+                if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
+                    motorValue = handleOutputScaling(
+                        motor[i],
+                        throttleRangeMin,
+                        motor[i],
+                        throttleRangeMin,
+                        throttleRangeMax,
+                        reversibleMotorsConfig()->deadband_high,
+                        motorConfig()->maxthrottle,
+                        true
+                    );
+                } else {
+                    motorValue = handleOutputScaling(
+                        motor[i],
+                        throttleRangeMax,
+                        motor[i],
+                        throttleRangeMin,
+                        throttleRangeMax,
+                        motorConfig()->mincommand,
+                        reversibleMotorsConfig()->deadband_low,
+                        false
+                    );
+                }
+            } else {
+                motorValue = motor[i];
+            }
+
         }
 #else
         // We don't define USE_DSHOT
@@ -335,7 +429,20 @@ void stopPwmAllMotors(void)
     pwmShutdownPulsesForAllMotors(motorCount);
 }
 
-void FAST_CODE NOINLINE mixTable(const float dT)
+static int getReversibleMotorsThrottleDeadband(void)
+{
+    int directionValue;
+
+    if (reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) {
+        directionValue = reversibleMotorsConfig()->deadband_low;
+    } else {
+        directionValue = reversibleMotorsConfig()->deadband_high;
+    }
+
+    return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue;
+}
+
+void FAST_CODE mixTable(const float dT)
 {
     int16_t input[3];   // RPY, range [-500:+500]
     // Allow direct stick input to motors in passthrough mode on airplanes
@@ -370,51 +477,57 @@ void FAST_CODE NOINLINE mixTable(const float dT)
     int16_t rpyMixRange = rpyMixMax - rpyMixMin;
     int16_t throttleRange;
     int16_t throttleMin, throttleMax;
-    static int16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
 
     // Find min and max throttle based on condition.
 #ifdef USE_GLOBAL_FUNCTIONS
     if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
-        throttleMin = throttleIdleValue;
-        throttleMax = motorConfig()->maxthrottle;
-        mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax); 
+        throttleRangeMin = throttleIdleValue;
+        throttleRangeMax = motorConfig()->maxthrottle;
+        mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); 
     } else
 #endif
     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
-        if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming.
 
-        if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Out of band handling
-            throttleMax = reversibleMotorsConfig()->deadband_low;
-            throttleMin = throttleIdleValue;
-            throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
-        } else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband)) { // Positive handling
-            throttleMax = motorConfig()->maxthrottle;
-            throttleMin = reversibleMotorsConfig()->deadband_high;
-            throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
-        } else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband)))  { // Deadband handling from negative to positive
-            mixerThrottleCommand = throttleMax = reversibleMotorsConfig()->deadband_low;
-            throttleMin = throttleIdleValue;
-        } else {  // Deadband handling from positive to negative
-            throttleMax = motorConfig()->maxthrottle;
-            mixerThrottleCommand = throttleMin = reversibleMotorsConfig()->deadband_high;
+        if (rcCommand[THROTTLE] >= (throttleDeadbandHigh) || STATE(SET_REVERSIBLE_MOTORS_FORWARD)) {
+            /*
+             * Throttle is above deadband, FORWARD direction
+             */
+            reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
+            throttleRangeMax = motorConfig()->maxthrottle;
+            throttleRangeMin = throttleDeadbandHigh;
+            DISABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
+        } else if (rcCommand[THROTTLE] <= throttleDeadbandLow) {
+            /*
+             * Throttle is below deadband, BACKWARD direction
+             */
+            reversibleMotorsThrottleState = MOTOR_DIRECTION_BACKWARD;
+            throttleRangeMax = throttleDeadbandLow;
+            throttleRangeMin = motorConfig()->mincommand;
         }
+
+
+        motorValueWhenStopped = getReversibleMotorsThrottleDeadband();
+        mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax);
     } else {
         mixerThrottleCommand = rcCommand[THROTTLE];
-        throttleMin = throttleIdleValue;
-        throttleMax = motorConfig()->maxthrottle;
+        throttleRangeMin = throttleIdleValue;
+        throttleRangeMax = motorConfig()->maxthrottle;
 
         // Throttle scaling to limit max throttle when battery is full
     #ifdef USE_GLOBAL_FUNCTIONS
-        mixerThrottleCommand = ((mixerThrottleCommand - throttleMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleMin;
+        mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleRangeMin;
     #else
-        mixerThrottleCommand = ((mixerThrottleCommand - throttleMin) * motorConfig()->throttleScale) + throttleMin;
+        mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin;
     #endif
         // Throttle compensation based on battery voltage
         if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {                
-            mixerThrottleCommand = MIN(throttleMin + (mixerThrottleCommand - throttleMin) * calculateThrottleCompensationFactor(), throttleMax);
+            mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax);
         }
     }
 
+    throttleMin = throttleRangeMin;
+    throttleMax = throttleRangeMax;
+
     throttleRange = throttleMax - throttleMin;
 
     #define THROTTLE_CLIPPING_FACTOR    0.33f
@@ -441,14 +554,8 @@ void FAST_CODE NOINLINE mixTable(const float dT)
 
             if (failsafeIsActive()) {
                 motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
-            } else if (feature(FEATURE_REVERSIBLE_MOTORS)) {
-                if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband)) {
-                    motor[i] = constrain(motor[i], throttleIdleValue, reversibleMotorsConfig()->deadband_low);
-                } else {
-                    motor[i] = constrain(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle);
-                }
             } else {
-                motor[i] = constrain(motor[i], throttleIdleValue, motorConfig()->maxthrottle);
+                motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
             }
 
             // Motor stop handling
@@ -472,7 +579,7 @@ motorStatus_e getMotorStatus(void)
         return MOTOR_STOPPED_AUTO;
     }
 
-    if (calculateThrottleStatus() == THROTTLE_LOW) {
+    if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) {
         if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
             return MOTOR_STOPPED_USER;
         }
@@ -485,4 +592,4 @@ void loadPrimaryMotorMixer(void) {
     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
         currentMixer[i] = *primaryMotorMixer(i);
     }
-}
\ No newline at end of file
+}
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 351a371e4d..405c876aa9 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -102,6 +102,12 @@ typedef enum {
     MOTOR_RUNNING
 } motorStatus_e;
 
+typedef enum {
+    MOTOR_DIRECTION_FORWARD,
+    MOTOR_DIRECTION_BACKWARD,
+    MOTOR_DIRECTION_DEADBAND
+} reversibleMotorsThrottleState_e;
+
 extern int16_t motor[MAX_SUPPORTED_MOTORS];
 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 extern int mixerThrottleCommand;
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 3897ecac33..7fba6b4b3c 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -21,6 +21,8 @@
 
 #include <platform.h>
 
+FILE_COMPILE_FOR_SPEED
+
 #include "build/build_config.h"
 #include "build/debug.h"
 
@@ -577,7 +579,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
 }
 
 /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
-static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
+static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 {
     const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch;
 
@@ -599,7 +601,7 @@ bool isFixedWingItermLimitActive(float stickPosition)
     return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
 }
 
-static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, float rateError, float dT) {
+static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
     float newPTerm = rateError * pidState->kP;
 
     return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
@@ -717,18 +719,18 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
         newDTerm = 0;
     } else {
         // Calculate delta for Dterm calculation. Apply filters before derivative to minimize effects of dterm kick
-        float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
-
-        // Apply D-term notch
-        deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
-
-        // Apply additional lowpass
-        deltaFiltered = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, deltaFiltered);
-        deltaFiltered = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, deltaFiltered);
+        const float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
 
         firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
         newDTerm = firFilterApply(&pidState->gyroRateFilter);
 
+        // Apply D-term notch
+        newDTerm = notchFilterApplyFn(&pidState->deltaNotchFilter, newDTerm);
+
+        // Apply additional lowpass
+        newDTerm = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, newDTerm);
+        newDTerm = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, newDTerm);
+
         // Calculate derivative
         newDTerm =  newDTerm * (pidState->kD / dT) * applyDBoost(pidState, dT);
 
@@ -943,7 +945,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
 }
 
-void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
+void checkItermLimitingActive(pidState_t *pidState)
 {
     bool shouldActivate;
     if (usedPidControllerType == PID_TYPE_PIFF) {
@@ -956,7 +958,7 @@ void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
     pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; 
 }
 
-void FAST_CODE NOINLINE pidController(float dT)
+void FAST_CODE pidController(float dT)
 {
     if (!pidFiltersConfigured) {
         return;
@@ -1122,9 +1124,9 @@ void pidInit(void)
     }
 }
 
-const pidBank_t FAST_CODE NOINLINE * pidBank(void) { 
+const pidBank_t * pidBank(void) { 
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; 
 }
-pidBank_t FAST_CODE NOINLINE * pidBankMutable(void) { 
+pidBank_t * pidBankMutable(void) { 
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 51e6469d9d..d4187ccf70 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -152,7 +152,7 @@ typedef struct pidProfile_s {
     float antigravityAccelerator;
     uint8_t antigravityCutoff;
 
-    int navFwPosHdgPidsumLimit;
+    uint16_t navFwPosHdgPidsumLimit;
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {
diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c
index 7d861de498..e2b2c9849d 100644
--- a/src/main/flight/rpm_filter.c
+++ b/src/main/flight/rpm_filter.c
@@ -22,6 +22,8 @@
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
 
+#include "platform.h"
+
 #include "flight/rpm_filter.h"
 
 #include "config/parameter_group.h"
@@ -82,7 +84,7 @@ void nullRpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseF
     UNUSED(baseFrequency);
 }
 
-float FAST_CODE rpmFilterApply(rpmFilterBank_t *filterBank, uint8_t axis, float input)
+float rpmFilterApply(rpmFilterBank_t *filterBank, uint8_t axis, float input)
 {
     float output = input;
 
@@ -136,7 +138,7 @@ void disableRpmFilters(void) {
     rpmGyroApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
 }
 
-void FAST_CODE NOINLINE rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
+void rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
 {
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
     {
@@ -177,7 +179,7 @@ void rpmFiltersInit(void)
     }
 }
 
-void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
+void rpmFilterUpdateTask(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
 
@@ -198,7 +200,7 @@ void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
     }
 }
 
-float FAST_CODE rpmFilterGyroApply(uint8_t axis, float input)
+float rpmFilterGyroApply(uint8_t axis, float input)
 {
     return rpmGyroApplyFn(&gyroRpmFilters, axis, input);
 }
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index dac370616b..375eacb288 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -28,6 +28,7 @@
 #include "common/axis.h"
 #include "common/filter.h"
 #include "common/maths.h"
+#include "common/global_variables.h"
 
 #include "config/config_reset.h"
 #include "config/feature.h"
@@ -35,6 +36,7 @@
 #include "config/parameter_group_ids.h"
 
 #include "drivers/pwm_output.h"
+#include "drivers/pwm_mapping.h"
 #include "drivers/time.h"
 
 #include "fc/config.h"
@@ -52,12 +54,13 @@
 
 #include "sensors/gyro.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1);
 
 PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
     .servoCenterPulse = 1500,
     .servoPwmRate = 50,             // Default for analog servos
     .servo_lowpass_freq = 20,       // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions
+    .servo_protocol = SERVO_TYPE_PWM,
     .flaperon_throw_offset = FLAPERON_THROW_DEFAULT,
     .tri_unarmed_servo = 1
 );
@@ -262,7 +265,13 @@ void servoMixer(float dT)
 
     input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0;
 
-    input[INPUT_LOGIC_ONE] = 500;
+    input[INPUT_MAX] = 500;
+#ifdef USE_LOGIC_CONDITIONS
+    input[INPUT_GVAR_0] = constrain(gvGet(0), -1000, 1000);
+    input[INPUT_GVAR_1] = constrain(gvGet(1), -1000, 1000);
+    input[INPUT_GVAR_2] = constrain(gvGet(2), -1000, 1000);
+    input[INPUT_GVAR_3] = constrain(gvGet(3), -1000, 1000);
+#endif
 
     if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
         input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -900, 900, -500, +500);
@@ -444,17 +453,17 @@ void processServoAutotrim(void)
     }
 }
 
-bool FAST_CODE NOINLINE isServoOutputEnabled(void)
+bool isServoOutputEnabled(void)
 {
     return servoOutputEnabled;
 }
 
-void NOINLINE setServoOutputEnabled(bool flag)
+void setServoOutputEnabled(bool flag)
 {
     servoOutputEnabled = flag;
 }
 
-bool FAST_CODE NOINLINE isMixerUsingServos(void)
+bool isMixerUsingServos(void)
 {
     return mixerUsesServos;
 }
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index 3025bc4edc..f8ca7b5112 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -53,7 +53,11 @@ typedef enum {
     INPUT_STABILIZED_PITCH_MINUS    = 26,
     INPUT_STABILIZED_YAW_PLUS       = 27,
     INPUT_STABILIZED_YAW_MINUS      = 28,
-    INPUT_LOGIC_ONE                 = 29,
+    INPUT_MAX                       = 29,
+    INPUT_GVAR_0                    = 30,
+    INPUT_GVAR_1                    = 31,
+    INPUT_GVAR_2                    = 32,
+    INPUT_GVAR_3                    = 33,
 
     INPUT_SOURCE_COUNT
 } inputSource_e;
@@ -128,7 +132,7 @@ typedef struct servoConfig_s {
     uint16_t servoPwmRate;                  // The update rate of servo outputs (50-498Hz)
     int16_t servo_lowpass_freq;             // lowpass servo filter frequency selection; 1/1000ths of loop freq
     uint16_t flaperon_throw_offset;
-    uint8_t __reserved;
+    uint8_t servo_protocol;                 // See servoProtocolType_e
     uint8_t tri_unarmed_servo;              // send tail servo correction pulses even when unarmed
 } servoConfig_t;
 
diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index 619fb552d7..f14673f0df 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -418,14 +418,6 @@ static void showStatusPage(void)
         i2c_OLED_send_string(lineBuffer);
     }
 #endif
-
-    rowIndex++;
-    char rollTrim[7], pitchTrim[7];
-    formatTrimDegrees(rollTrim, boardAlignment()->rollDeciDegrees);
-    formatTrimDegrees(pitchTrim, boardAlignment()->pitchDeciDegrees);
-    tfp_sprintf(lineBuffer, "Acc: %sR, %sP", rollTrim, pitchTrim );
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
 }
 
 void dashboardUpdate(timeUs_t currentTimeUs)
@@ -551,16 +543,4 @@ void dashboardSetNextPageChangeAt(timeUs_t futureMicros)
     nextPageAt = futureMicros;
 }
 
-void formatTrimDegrees ( char *formattedTrim, int16_t trimValue ) {
-    char trim[6];
-    tfp_sprintf(trim, "%d", trimValue);
-    int x = strlen(trim)-1;
-    strncpy(formattedTrim,trim,x);
-    formattedTrim[x] = '\0';
-    if (trimValue !=0) {
-        strcat(formattedTrim,".");
-    }
-    strcat(formattedTrim,trim+x);
-}
-
 #endif
diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h
index 91c3418395..06186e7eed 100644
--- a/src/main/io/dashboard.h
+++ b/src/main/io/dashboard.h
@@ -25,6 +25,4 @@ void dashboardInit(void);
 void dashboardUpdate(timeUs_t currentTimeUs);
 
 void dashboardSetPage(pageId_e newPageId);
-void dashboardSetNextPageChangeAt(timeUs_t futureMicros);
-
-void formatTrimDegrees ( char formattedTrim[7], int16_t trimValue );
+void dashboardSetNextPageChangeAt(timeUs_t futureMicros);
\ No newline at end of file
diff --git a/src/main/io/displayport_frsky_osd.c b/src/main/io/displayport_frsky_osd.c
index 8aa0b26061..f5b02d6a50 100644
--- a/src/main/io/displayport_frsky_osd.c
+++ b/src/main/io/displayport_frsky_osd.c
@@ -259,14 +259,14 @@ static void setLineOutlineType(displayCanvas_t *displayCanvas, displayCanvasOutl
 {
     UNUSED(displayCanvas);
 
-    frskyOSDSetLineOutlineType(outlineType);
+    frskyOSDSetLineOutlineType((frskyOSDLineOutlineType_e)outlineType);
 }
 
 static void setLineOutlineColor(displayCanvas_t *displayCanvas, displayCanvasColor_e outlineColor)
 {
     UNUSED(displayCanvas);
 
-    frskyOSDSetLineOutlineColor(outlineColor);
+    frskyOSDSetLineOutlineColor((frskyOSDColor_e)outlineColor);
 }
 
 static void clipToRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)
diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c
index 545d136cd8..bb50b269f2 100644
--- a/src/main/io/flashfs.c
+++ b/src/main/io/flashfs.c
@@ -24,18 +24,17 @@
  *
  * Note that bits can only be set to 0 when writing, not back to 1 from 0. You must erase sectors in order
  * to bring bits back to 1 again.
- *
- * In future, we can add support for multiple different flash chips by adding a flash device driver vtable
- * and make calls through that, at the moment flashfs just calls m25p16_* routines explicitly.
  */
 
 #include <stdint.h>
 #include <stdbool.h>
 #include <string.h>
 
-#include "drivers/flash_m25p16.h"
+#include "drivers/flash.h"
 #include "flashfs.h"
 
+static flashPartition_t *flashPartition;
+
 static uint8_t flashWriteBuffer[FLASHFS_WRITE_BUFFER_SIZE];
 
 /* The position of our head and tail in the circular flash write buffer.
@@ -67,10 +66,8 @@ static void flashfsSetTailAddress(uint32_t address)
 
 void flashfsEraseCompletely(void)
 {
-    m25p16_eraseCompletely();
-
+    flashPartitionErase(flashPartition);
     flashfsClearBuffer();
-
     flashfsSetTailAddress(0);
 }
 
@@ -80,7 +77,7 @@ void flashfsEraseCompletely(void)
  */
 void flashfsEraseRange(uint32_t start, uint32_t end)
 {
-    const flashGeometry_t *geometry = m25p16_getGeometry();
+    const flashGeometry_t *geometry = flashGetGeometry();
 
     if (geometry->sectorSize <= 0)
         return;
@@ -97,7 +94,7 @@ void flashfsEraseRange(uint32_t start, uint32_t end)
     }
 
     for (int i = startSector; i < endSector; i++) {
-        m25p16_eraseSector(i * geometry->sectorSize);
+        flashEraseSector(i * geometry->sectorSize);
     }
 }
 
@@ -106,12 +103,12 @@ void flashfsEraseRange(uint32_t start, uint32_t end)
  */
 bool flashfsIsReady(void)
 {
-    return m25p16_isReady();
+    return !!flashPartition;
 }
 
 uint32_t flashfsGetSize(void)
 {
-    return m25p16_getGeometry()->totalSize;
+    return flashPartitionSize(flashPartition);
 }
 
 static uint32_t flashfsTransmitBufferUsed(void)
@@ -138,11 +135,6 @@ uint32_t flashfsGetWriteBufferFreeSpace(void)
     return flashfsGetWriteBufferSize() - flashfsTransmitBufferUsed();
 }
 
-const flashGeometry_t* flashfsGetGeometry(void)
-{
-    return m25p16_getGeometry();
-}
-
 /**
  * Write the given buffers to flash sequentially at the current tail address, advancing the tail address after
  * each write.
@@ -164,6 +156,8 @@ const flashGeometry_t* flashfsGetGeometry(void)
  */
 static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSizes, int bufferCount, bool sync)
 {
+    const flashGeometry_t *geometry = flashGetGeometry();
+
     uint32_t bytesTotal = 0;
 
     int i;
@@ -172,7 +166,7 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
         bytesTotal += bufferSizes[i];
     }
 
-    if (!sync && !m25p16_isReady()) {
+    if (!sync && !flashIsReady()) {
         return 0;
     }
 
@@ -187,8 +181,8 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
          * Each page needs to be saved in a separate program operation, so
          * if we would cross a page boundary, only write up to the boundary in this iteration:
          */
-        if (tailAddress % M25P16_PAGESIZE + bytesTotalRemaining > M25P16_PAGESIZE) {
-            bytesTotalThisIteration = M25P16_PAGESIZE - tailAddress % M25P16_PAGESIZE;
+        if (tailAddress % geometry->pageSize + bytesTotalRemaining > geometry->pageSize) {
+            bytesTotalThisIteration = geometry->pageSize - tailAddress % geometry->pageSize;
         } else {
             bytesTotalThisIteration = bytesTotalRemaining;
         }
@@ -207,7 +201,7 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
             if (bufferSizes[i] > 0) {
                 // Is buffer larger than our write limit? Write our limit out of it
                 if (bufferSizes[i] >= bytesRemainThisIteration) {
-                    currentFlashAddress = m25p16_pageProgram(currentFlashAddress, buffers[i], bytesRemainThisIteration);
+                    currentFlashAddress = flashPageProgram(currentFlashAddress, buffers[i], bytesRemainThisIteration);
 
                     buffers[i] += bytesRemainThisIteration;
                     bufferSizes[i] -= bytesRemainThisIteration;
@@ -216,7 +210,7 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
                     break;
                 } else {
                     // We'll still have more to write after finishing this buffer off
-                    currentFlashAddress = m25p16_pageProgram(currentFlashAddress, buffers[i], bufferSizes[i]);
+                    currentFlashAddress = flashPageProgram(currentFlashAddress, buffers[i], bufferSizes[i]);
 
                     bytesRemainThisIteration -= bufferSizes[i];
 
@@ -471,7 +465,7 @@ int flashfsReadAbs(uint32_t address, uint8_t *buffer, unsigned int len)
     // Since the read could overlap data in our dirty buffers, force a sync to clear those first
     flashfsFlushSync();
 
-    bytesRead = m25p16_readBytes(address, buffer, len);
+    bytesRead = flashReadBytes(address, buffer, len);
 
     return bytesRead;
 }
@@ -516,7 +510,7 @@ int flashfsIdentifyStartOfFreeSpace(void)
     while (left < right) {
         mid = (left + right) / 2;
 
-        if (m25p16_readBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES) < FREE_BLOCK_TEST_SIZE_BYTES) {
+        if (flashReadBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES) < FREE_BLOCK_TEST_SIZE_BYTES) {
             // Unexpected timeout from flash, so bail early (reporting the device fuller than it really is)
             break;
         }
@@ -558,8 +552,9 @@ bool flashfsIsEOF(void)
  */
 void flashfsInit(void)
 {
-    // If we have a flash chip present at all
-    if (flashfsGetSize() > 0) {
+    flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FLASHFS);
+
+    if (flashPartition) {
         // Start the file pointer off at the beginning of free space so caller can start writing immediately
         flashfsSeekAbs(flashfsIdentifyStartOfFreeSpace());
     }
diff --git a/src/main/io/flashfs.h b/src/main/io/flashfs.h
index 02b79d69c3..4e14692ed2 100644
--- a/src/main/io/flashfs.h
+++ b/src/main/io/flashfs.h
@@ -35,7 +35,6 @@ uint32_t flashfsGetOffset(void);
 uint32_t flashfsGetWriteBufferFreeSpace(void);
 uint32_t flashfsGetWriteBufferSize(void);
 int flashfsIdentifyStartOfFreeSpace(void);
-const flashGeometry_t* flashfsGetGeometry(void);
 
 void flashfsSeekAbs(uint32_t offset);
 void flashfsSeekRel(int32_t offset);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 220bf8e50f..f7a0f2d943 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -31,6 +31,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #ifdef USE_OSD
 
 #include "build/debug.h"
@@ -196,7 +198,7 @@ static bool osdDisplayHasCanvas;
 #define AH_SIDEBAR_WIDTH_POS 7
 #define AH_SIDEBAR_HEIGHT_POS 3
 
-PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 9);
+PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12);
 
 static int digitCount(int32_t value)
 {
@@ -787,11 +789,12 @@ static const char * navigationStateMessage(void)
             // Used by HOLD flight modes. No information to add.
             break;
         case MW_NAV_STATE_HOLD_TIMED:
-            // Not used anymore
+            // TODO: Maybe we can display a count down
+            return OSD_MESSAGE_STR("HOLDING WAYPOINT");
             break;
         case MW_NAV_STATE_WP_ENROUTE:
             // TODO: Show WP number
-            return OSD_MESSAGE_STR("EN ROUTE TO WAYPOINT");
+            return OSD_MESSAGE_STR("TO WP");
         case MW_NAV_STATE_PROCESS_NEXT:
             return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
         case MW_NAV_STATE_DO_JUMP:
@@ -1638,13 +1641,14 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "3CRS";
             else if (FLIGHT_MODE(NAV_CRUISE_MODE))
                 p = "CRS ";
+            else if (FLIGHT_MODE(NAV_WP_MODE))
+                p = " WP ";
             else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
                 // If navigationRequiresAngleMode() returns false when ALTHOLD is active,
                 // it means it can be combined with ANGLE, HORIZON, ACRO, etc...
                 // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
                 p = " AH ";
-            } else if (FLIGHT_MODE(NAV_WP_MODE))
-                p = " WP ";
+            }
             else if (FLIGHT_MODE(ANGLE_MODE))
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
@@ -1711,14 +1715,18 @@ static bool osdDrawSingleElement(uint8_t item)
 
         if (STATE(GPS_FIX) && isImuHeadingValid()) {
 
-            if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0) {
+            if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
                     osdHudClear();
             }
 
+            // -------- POI : Home point
+
             if (osdConfig()->hud_homepoint) { // Display the home point (H)
-                osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, 5, SYM_HOME);
+                osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, SYM_HOME, 0 , 0);
             }
 
+            // -------- POI : Nearby aircrafts from ESP32 radar
+
             if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar
                 for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) {
                     if (radar_pois[i].gps.lat != 0 && radar_pois[i].gps.lon != 0 && radar_pois[i].state < 2) { // state 2 means POI has been lost and must be skipped
@@ -1729,7 +1737,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         if (radar_pois[i].distance >= osdConfig()->hud_radar_range_min && radar_pois[i].distance <= osdConfig()->hud_radar_range_max) {
                             radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In °
                             radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100;
-                            osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, radar_pois[i].heading, radar_pois[i].lq, 65 + i);
+                            osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, 1, 65 + i, radar_pois[i].heading, radar_pois[i].lq);
                         }
                     }
                 }
@@ -1741,6 +1749,29 @@ static bool osdDrawSingleElement(uint8_t item)
                     }
                 }
             }
+
+            // -------- POI : Next waypoints from navigation
+
+            if (osdConfig()->hud_wp_disp > 0 && posControl.waypointListValid && posControl.waypointCount > 0) { // Display the next waypoints
+                gpsLocation_t wp2;
+                int j;
+
+                tfp_sprintf(buff, "W%u/%u", posControl.activeWaypointIndex, posControl.waypointCount);
+                displayWrite(osdGetDisplayPort(), 13, osdConfig()->hud_margin_v - 1, buff);
+
+                for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
+                    j = posControl.activeWaypointIndex + i;
+                    if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) {
+                        wp2.lat = posControl.waypointList[j].lat;
+                        wp2.lon = posControl.waypointList[j].lon;
+                        wp2.alt = posControl.waypointList[j].alt;
+                        fpVector3_t poi;
+                        geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE);
+                        while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
+                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i);
+                    }
+                }
+            }
         }
 
         return true;
@@ -2449,6 +2480,13 @@ static bool osdDrawSingleElement(uint8_t item)
             }
             break;
         }
+    case OSD_ESC_TEMPERATURE:
+        {
+            escSensorData_t * escSensor = escSensorGetData();
+            bool escTemperatureValid = escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE;
+            osdDisplayTemperature(elemPosX, elemPosY, SYM_ESC_TEMP, NULL, escTemperatureValid, (escSensor->temperature)*10, osdConfig()->esc_temp_alarm_min, osdConfig()->esc_temp_alarm_max);
+            return true;
+        }
 #endif
 
     default:
@@ -2664,6 +2702,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
 
 #if defined(USE_ESC_SENSOR)
     osdConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2);
+    osdConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3);
 #endif
 
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
@@ -2687,6 +2726,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->current_alarm = 0;
     osdConfig->imu_temp_alarm_min = -200;
     osdConfig->imu_temp_alarm_max = 600;
+    osdConfig->esc_temp_alarm_min = -200;
+    osdConfig->esc_temp_alarm_max = 900;
     osdConfig->gforce_alarm = 5;
     osdConfig->gforce_axis_alarm_min = -5;
     osdConfig->gforce_axis_alarm_max = 5;
@@ -2713,9 +2754,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->hud_homing = 0;
     osdConfig->hud_homepoint = 0;
     osdConfig->hud_radar_disp = 0;
-    osdConfig->hud_radar_range_min = 10;
+    osdConfig->hud_radar_range_min = 3;
     osdConfig->hud_radar_range_max = 4000;
     osdConfig->hud_radar_nearest = 0;
+    osdConfig->hud_wp_disp = 0;
     osdConfig->left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
     osdConfig->right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
     osdConfig->sidebar_scroll_arrows = 0;
@@ -2904,6 +2946,7 @@ static void osdShowStats(void)
     const uint8_t statValuesX = 20;
     char buff[10];
 
+    displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
     displayClearScreen(osdDisplayPort);
     if (IS_DISPLAY_PAL)
         displayWrite(osdDisplayPort, statNameX, top++, "  --- STATS ---");
@@ -2999,6 +3042,7 @@ static void osdShowStats(void)
 
     displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY      :");
     displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
+    displayCommitTransaction(osdDisplayPort);
 }
 
 // called when motors armed
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 6205e772c5..00dc2fd6d7 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -151,6 +151,7 @@ typedef enum {
     OSD_RC_SOURCE,
     OSD_VTX_POWER,
     OSD_ESC_RPM,
+    OSD_ESC_TEMPERATURE,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -187,6 +188,11 @@ typedef enum {
     OSD_ALIGN_RIGHT
 } osd_alignment_e;
 
+typedef enum {
+    OSD_AHI_STYLE_DEFAULT,
+    OSD_AHI_STYLE_LINE,
+} osd_ahi_style_e;
+
 typedef struct osdConfig_s {
     // Layouts
     uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT];
@@ -200,6 +206,8 @@ typedef struct osdConfig_s {
     uint8_t current_alarm; // current consumption in A
     int16_t imu_temp_alarm_min;
     int16_t imu_temp_alarm_max;
+    int16_t esc_temp_alarm_min;
+    int16_t esc_temp_alarm_max;
     float gforce_alarm;
     float gforce_axis_alarm_min;
     float gforce_axis_alarm_max;
@@ -231,6 +239,7 @@ typedef struct osdConfig_s {
     uint16_t hud_radar_range_min;
     uint16_t hud_radar_range_max;
     uint16_t hud_radar_nearest;
+    uint8_t hud_wp_disp;
     
     uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e
     uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e
@@ -244,6 +253,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 osd_ahi_style;
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);
diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c
index c4d562be45..4d42b6daf4 100644
--- a/src/main/io/osd_canvas.c
+++ b/src/main/io/osd_canvas.c
@@ -32,6 +32,7 @@
 
 #define AHI_MIN_DRAW_INTERVAL_MS 100
 #define AHI_MAX_DRAW_INTERVAL_MS 1000
+#define AHI_CROSSHAIR_MARGIN 6
 
 #include "common/log.h"
 #include "common/maths.h"
@@ -45,6 +46,7 @@
 #include "drivers/time.h"
 
 #include "io/osd_common.h"
+#include "io/osd.h"
 
 void osdCanvasDrawVarioShape(displayCanvas_t *canvas, unsigned ex, unsigned ey, float zvel, bool erase)
 {
@@ -162,7 +164,6 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA
 {
     int barWidth = (OSD_AHI_WIDTH - 1) * canvas->gridElementWidth;
     int levelBarWidth = barWidth * (3.0/4);
-    int crosshairMargin = 6;
     float pixelsPerDegreeLevel = 3.5f;
     int borderSize = 3;
     char buf[12];
@@ -220,9 +221,9 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA
         if (ii == 0) {
             displayCanvasSetLineOutlineType(canvas, DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM);
             displayCanvasMoveToPoint(canvas, -barWidth / 2, 0);
-            displayCanvasStrokeLineToPoint(canvas, -crosshairMargin, 0);
+            displayCanvasStrokeLineToPoint(canvas, -AHI_CROSSHAIR_MARGIN, 0);
             displayCanvasMoveToPoint(canvas, barWidth / 2, 0);
-            displayCanvasStrokeLineToPoint(canvas, crosshairMargin, 0);
+            displayCanvasStrokeLineToPoint(canvas, AHI_CROSSHAIR_MARGIN, 0);
             continue;
         }
 
@@ -257,6 +258,48 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA
     displayCanvasContextPop(canvas);
 }
 
+void osdDrawArtificialHorizonLine(displayCanvas_t *canvas, float pitchAngle, float rollAngle, bool erase)
+{
+    int barWidth = (OSD_AHI_WIDTH - 1) * canvas->gridElementWidth;
+    int maxHeight = canvas->height;
+    float pixelsPerDegreeLevel = 1.5f;
+    int maxWidth = (OSD_AHI_WIDTH + 1) * canvas->gridElementWidth;
+
+    int lx = (canvas->width - maxWidth) / 2;
+
+    displayCanvasContextPush(canvas);
+
+    displayCanvasClipToRect(canvas, lx, 0, maxWidth, maxHeight);
+    osdGridBufferClearPixelRect(canvas, lx, 0, maxWidth, maxHeight);
+
+    if (erase) {
+        displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
+        displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
+    } else {
+        displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_WHITE);
+        displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
+    }
+
+    float pitchDegrees = RADIANS_TO_DEGREES(pitchAngle);
+    float pitchOffset = -pitchDegrees * pixelsPerDegreeLevel;
+    float translateX = canvas->width / 2;
+    float translateY = canvas->height / 2;
+
+    displayCanvasCtmTranslate(canvas, 0, pitchOffset);
+    displayCanvasCtmRotate(canvas, rollAngle);
+    displayCanvasCtmTranslate(canvas, translateX, translateY);
+
+
+    displayCanvasSetStrokeWidth(canvas, 2);
+    displayCanvasSetLineOutlineType(canvas, DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM);
+    displayCanvasMoveToPoint(canvas, -barWidth / 2, 0);
+    displayCanvasStrokeLineToPoint(canvas, -AHI_CROSSHAIR_MARGIN, 0);
+    displayCanvasMoveToPoint(canvas, barWidth / 2, 0);
+    displayCanvasStrokeLineToPoint(canvas, AHI_CROSSHAIR_MARGIN, 0);
+
+    displayCanvasContextPop(canvas);
+}
+
 void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float pitchAngle, float rollAngle)
 {
     UNUSED(display);
@@ -271,12 +314,20 @@ void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *can
 
     float totalError = fabsf(prevPitchAngle - pitchAngle) + fabsf(prevRollAngle - rollAngle);
     if ((now > nextDrawMinMs && totalError > 0.05f)|| now > nextDrawMaxMs) {
-
-        int x, y, w, h;
-        osdArtificialHorizonRect(canvas, &x, &y, &w, &h);
-        displayCanvasClearRect(canvas, x, y, w, h);
-
-        osdDrawArtificialHorizonShapes(canvas, pitchAngle, rollAngle);
+        switch ((osd_ahi_style_e)osdConfig()->osd_ahi_style) {
+            case OSD_AHI_STYLE_DEFAULT:
+            {
+                int x, y, w, h;
+                osdArtificialHorizonRect(canvas, &x, &y, &w, &h);
+                displayCanvasClearRect(canvas, x, y, w, h);
+                osdDrawArtificialHorizonShapes(canvas, pitchAngle, rollAngle);
+                break;
+            }
+            case OSD_AHI_STYLE_LINE:
+                osdDrawArtificialHorizonLine(canvas, prevPitchAngle, prevRollAngle, true);
+                osdDrawArtificialHorizonLine(canvas, pitchAngle, rollAngle, false);
+                break;
+        }
         prevPitchAngle = pitchAngle;
         prevRollAngle = rollAngle;
         nextDrawMinMs = now + AHI_MIN_DRAW_INTERVAL_MS;
diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c
index 38e7bf52ce..f8f2dd8b0a 100644
--- a/src/main/io/osd_dji_hd.c
+++ b/src/main/io/osd_dji_hd.c
@@ -104,64 +104,69 @@ const uint8_t djiPidIndexMap[] = {
     PID_HEADING // DJI: PID_MAG
 };
 
-const int djiOSDItemIndexMap[] = {
-    OSD_RSSI_VALUE,             // DJI: OSD_RSSI_VALUE
-    OSD_MAIN_BATT_VOLTAGE,      // DJI: OSD_MAIN_BATT_VOLTAGE
-    OSD_CROSSHAIRS,             // DJI: OSD_CROSSHAIRS
-    OSD_ARTIFICIAL_HORIZON,     // DJI: OSD_ARTIFICIAL_HORIZON
-    OSD_HORIZON_SIDEBARS,       // DJI: OSD_HORIZON_SIDEBARS
-    OSD_ONTIME,                 // DJI: OSD_ITEM_TIMER_1
-    OSD_FLYTIME,                // DJI: OSD_ITEM_TIMER_2
-    OSD_FLYMODE,                // DJI: OSD_FLYMODE
-    OSD_CRAFT_NAME,             // DJI: OSD_CRAFT_NAME
-    OSD_THROTTLE_POS,           // DJI: OSD_THROTTLE_POS
-    OSD_VTX_CHANNEL,            // DJI: OSD_VTX_CHANNEL
-    OSD_CURRENT_DRAW,           // DJI: OSD_CURRENT_DRAW
-    OSD_MAH_DRAWN,              // DJI: OSD_MAH_DRAWN
-    OSD_GPS_SPEED,              // DJI: OSD_GPS_SPEED
-    OSD_GPS_SATS,               // DJI: OSD_GPS_SATS
-    OSD_ALTITUDE,               // DJI: OSD_ALTITUDE
-    OSD_ROLL_PIDS,              // DJI: OSD_ROLL_PIDS
-    OSD_PITCH_PIDS,             // DJI: OSD_PITCH_PIDS
-    OSD_YAW_PIDS,               // DJI: OSD_YAW_PIDS
-    OSD_POWER,                  // DJI: OSD_POWER
-    -1,                         // DJI: OSD_PIDRATE_PROFILE
-    -1,                         // DJI: OSD_WARNINGS
-    OSD_MAIN_BATT_CELL_VOLTAGE, // DJI: OSD_AVG_CELL_VOLTAGE
-    OSD_GPS_LON,                // DJI: OSD_GPS_LON
-    OSD_GPS_LAT,                // DJI: OSD_GPS_LAT
-    OSD_DEBUG,                  // DJI: OSD_DEBUG
-    OSD_ATTITUDE_PITCH,         // DJI: OSD_PITCH_ANGLE
-    OSD_ATTITUDE_ROLL,          // DJI: OSD_ROLL_ANGLE
-    -1,                         // DJI: OSD_MAIN_BATT_USAGE
-    -1,                         // DJI: OSD_DISARMED
-    OSD_HOME_DIR,               // DJI: OSD_HOME_DIR
-    OSD_HOME_DIST,              // DJI: OSD_HOME_DIST
-    OSD_HEADING,                // DJI: OSD_NUMERICAL_HEADING
-    OSD_VARIO_NUM,              // DJI: OSD_NUMERICAL_VARIO
-    -1,                         // DJI: OSD_COMPASS_BAR
-    -1,                         // DJI: OSD_ESC_TMP
-    OSD_ESC_RPM,                // DJI: OSD_ESC_RPM
-    OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,   // DJI: OSD_REMAINING_TIME_ESTIMATE
-    OSD_RTC_TIME,               // DJI: OSD_RTC_DATETIME
-    -1,                         // DJI: OSD_ADJUSTMENT_RANGE
-    -1,                         // DJI: OSD_CORE_TEMPERATURE
-    -1,                         // DJI: OSD_ANTI_GRAVITY
-    -1,                         // DJI: OSD_G_FORCE
-    -1,                         // DJI: OSD_MOTOR_DIAG
-    -1,                         // DJI: OSD_LOG_STATUS
-    -1,                         // DJI: OSD_FLIP_ARROW
-    -1,                         // DJI: OSD_LINK_QUALITY
-    OSD_TRIP_DIST,              // DJI: OSD_FLIGHT_DIST
-    -1,                         // DJI: OSD_STICK_OVERLAY_LEFT
-    -1,                         // DJI: OSD_STICK_OVERLAY_RIGHT
-    -1,                         // DJI: OSD_DISPLAY_NAME
-    -1,                         // DJI: OSD_ESC_RPM_FREQ
-    -1,                         // DJI: OSD_RATE_PROFILE_NAME
-    -1,                         // DJI: OSD_PID_PROFILE_NAME
-    -1,                         // DJI: OSD_PROFILE_NAME
-    -1,                         // DJI: OSD_RSSI_DBM_VALUE
-    -1,                         // DJI: OSD_RC_CHANNELS
+typedef struct {
+    int         itemIndex;      // INAV OSD item
+    features_e  depFeature;     // INAV feature that item is dependent on
+} djiOsdMapping_t;
+
+const djiOsdMapping_t djiOSDItemIndexMap[] = {
+    { OSD_RSSI_VALUE,                         0 }, // DJI: OSD_RSSI_VALUE
+    { OSD_MAIN_BATT_VOLTAGE,                  FEATURE_VBAT }, // DJI: OSD_MAIN_BATT_VOLTAGE
+    { OSD_CROSSHAIRS,                         0 }, // DJI: OSD_CROSSHAIRS
+    { OSD_ARTIFICIAL_HORIZON,                 0 }, // DJI: OSD_ARTIFICIAL_HORIZON
+    { OSD_HORIZON_SIDEBARS,                   0 }, // DJI: OSD_HORIZON_SIDEBARS
+    { OSD_ONTIME,                             0 }, // DJI: OSD_ITEM_TIMER_1
+    { OSD_FLYTIME,                            0 }, // DJI: OSD_ITEM_TIMER_2
+    { OSD_FLYMODE,                            0 }, // DJI: OSD_FLYMODE
+    { OSD_CRAFT_NAME,                         0 }, // DJI: OSD_CRAFT_NAME
+    { OSD_THROTTLE_POS,                       0 }, // DJI: OSD_THROTTLE_POS
+    { OSD_VTX_CHANNEL,                        0 }, // DJI: OSD_VTX_CHANNEL
+    { OSD_CURRENT_DRAW,                       FEATURE_CURRENT_METER }, // DJI: OSD_CURRENT_DRAW
+    { OSD_MAH_DRAWN,                          FEATURE_CURRENT_METER }, // DJI: OSD_MAH_DRAWN
+    { OSD_GPS_SPEED,                          FEATURE_GPS }, // DJI: OSD_GPS_SPEED
+    { OSD_GPS_SATS,                           FEATURE_GPS }, // DJI: OSD_GPS_SATS
+    { OSD_ALTITUDE,                           0 }, // DJI: OSD_ALTITUDE
+    { OSD_ROLL_PIDS,                          0 }, // DJI: OSD_ROLL_PIDS
+    { OSD_PITCH_PIDS,                         0 }, // DJI: OSD_PITCH_PIDS
+    { OSD_YAW_PIDS,                           0 }, // DJI: OSD_YAW_PIDS
+    { OSD_POWER,                              0 }, // DJI: OSD_POWER
+    { -1,                                     0 }, // DJI: OSD_PIDRATE_PROFILE
+    { -1,                                     0 }, // DJI: OSD_WARNINGS
+    { OSD_MAIN_BATT_CELL_VOLTAGE,             0 }, // DJI: OSD_AVG_CELL_VOLTAGE
+    { OSD_GPS_LON,                            FEATURE_GPS }, // DJI: OSD_GPS_LON
+    { OSD_GPS_LAT,                            FEATURE_GPS }, // DJI: OSD_GPS_LAT
+    { OSD_DEBUG,                              0 }, // DJI: OSD_DEBUG
+    { OSD_ATTITUDE_PITCH,                     0 }, // DJI: OSD_PITCH_ANGLE
+    { OSD_ATTITUDE_ROLL,                      0 }, // DJI: OSD_ROLL_ANGLE
+    { -1,                                     0 }, // DJI: OSD_MAIN_BATT_USAGE
+    { -1,                                     0 }, // DJI: OSD_DISARMED
+    { OSD_HOME_DIR,                           FEATURE_GPS }, // DJI: OSD_HOME_DIR
+    { OSD_HOME_DIST,                          FEATURE_GPS }, // DJI: OSD_HOME_DIST
+    { OSD_HEADING,                            0 }, // DJI: OSD_NUMERICAL_HEADING
+    { OSD_VARIO_NUM,                          0 }, // DJI: OSD_NUMERICAL_VARIO
+    { -1,                                     0 }, // DJI: OSD_COMPASS_BAR
+    { -1,                                     0 }, // DJI: OSD_ESC_TMP
+    { OSD_ESC_RPM,                            0 }, // DJI: OSD_ESC_RPM
+    { OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,   FEATURE_CURRENT_METER }, // DJI: OSD_REMAINING_TIME_ESTIMATE
+    { OSD_RTC_TIME,                           0 }, // DJI: OSD_RTC_DATETIME
+    { -1,                                     0 }, // DJI: OSD_ADJUSTMENT_RANGE
+    { -1,                                     0 }, // DJI: OSD_CORE_TEMPERATURE
+    { -1,                                     0 }, // DJI: OSD_ANTI_GRAVITY
+    { -1,                                     0 }, // DJI: OSD_G_FORCE
+    { -1,                                     0 }, // DJI: OSD_MOTOR_DIAG
+    { -1,                                     0 }, // DJI: OSD_LOG_STATUS
+    { -1,                                     0 }, // DJI: OSD_FLIP_ARROW
+    { -1,                                     0 }, // DJI: OSD_LINK_QUALITY
+    { OSD_TRIP_DIST,                          FEATURE_GPS }, // DJI: OSD_FLIGHT_DIST
+    { -1,                                     0 }, // DJI: OSD_STICK_OVERLAY_LEFT
+    { -1,                                     0 }, // DJI: OSD_STICK_OVERLAY_RIGHT
+    { -1,                                     0 }, // DJI: OSD_DISPLAY_NAME
+    { -1,                                     0 }, // DJI: OSD_ESC_RPM_FREQ
+    { -1,                                     0 }, // DJI: OSD_RATE_PROFILE_NAME
+    { -1,                                     0 }, // DJI: OSD_PID_PROFILE_NAME
+    { -1,                                     0 }, // DJI: OSD_PROFILE_NAME
+    { -1,                                     0 }, // DJI: OSD_RSSI_DBM_VALUE
+    { -1,                                     0 }, // DJI: OSD_RC_CHANNELS
 };
 
 const int djiOSDStatisticsMap[] = {
@@ -258,6 +263,7 @@ static uint32_t djiPackArmingDisabledFlags(void)
     return isArmingDisabled() ? (1 << 24) : 0;
 }
 
+#if defined(USE_OSD)
 static uint32_t djiEncodeOSDEnabledWarnings(void)
 {
     // TODO:
@@ -288,8 +294,12 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
 
     // OSD element position and visibility
     for (unsigned i = 0; i < ARRAYLEN(djiOSDItemIndexMap); i++) {
-        int inavOSDIdx = djiOSDItemIndexMap[i];
-        if (inavOSDIdx >= 0) {
+        const int inavOSDIdx = djiOSDItemIndexMap[i].itemIndex;
+
+        // We call OSD item supported if it doesn't have dependencies or all feature-dependencies are satistied
+        const bool itemIsSupported = ((djiOSDItemIndexMap[i].depFeature == 0) || feature(djiOSDItemIndexMap[i].depFeature));
+
+        if (inavOSDIdx >= 0 && itemIsSupported) {
             // Position & visibility encoded in 16 bits. Position encoding is the same between BF/DJI and INAV
             // However visibility is different. INAV has 3 layouts, while BF only has visibility profiles
             // Here we use only one OSD layout mapped to first OSD BF profile
@@ -364,6 +374,7 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
     //sbufWriteU8(dst, DJI_OSD_SCREEN_WIDTH); // osdConfig()->camera_frame_width
     //sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height
 }
+#endif
 
 static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
 {
diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c
index 35c1f58078..535bb32c9e 100644
--- a/src/main/io/osd_hud.c
+++ b/src/main/io/osd_hud.c
@@ -36,7 +36,7 @@
 
 #ifdef USE_OSD
 
-#define HUD_DRAWN_MAXCHARS 40 // 6 POI (1 home, 5 radar) x 7 chars max for each minus 2 for H (no icons for heading and signal)
+#define HUD_DRAWN_MAXCHARS 54 // 8 POI (1 home, 4 radar, 3 WP) x 7 chars max for each, minus 2 for H
 
 static int8_t hud_drawn[HUD_DRAWN_MAXCHARS][2];
 static int8_t hud_drawn_pt;
@@ -113,16 +113,20 @@ int8_t radarGetNearestPOI(void)
 }
 
 /*
- * Display one POI on the hud, centered on crosshair position.
- * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°), Signal 0 to 5, Symbol 0 to 480
+ * Display a POI as a 3D-marker on the hud
+ * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°),
+ * Type = 0 : Home point
+ * Type = 1 : Radar POI, P1: Heading, P2: Signal
+ * Type = 2 : Waypoint, P1: WP number, P2: 1=WP+1, 2=WP+2, 3=WP+3
  */
-void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol)
+void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2)
 {
     int poi_x;
     int poi_y;
     uint8_t center_x;
     uint8_t center_y;
     bool poi_is_oos = 0;
+    char buff[3];
 
     uint8_t minX = osdConfig()->hud_margin_h + 2;
     uint8_t maxX = osdGetDisplayPort()->cols - osdConfig()->hud_margin_h - 3;
@@ -161,10 +165,10 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
         uint16_t c;
 
         poi_x = (error_x > 0 ) ? maxX : minX;
-        poi_y = center_y;
+        poi_y = center_y - 1;
 
         if (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK) {
-            poi_y = center_y - 2;
+            poi_y = center_y - 3;
             while (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK && poi_y < maxY - 3) { // Stacks the out-of-sight POI from top to bottom
                 poi_y += 2;
             }
@@ -180,21 +184,22 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
         }
     }
 
-    // POI marker (A B C ...)
+    // Markers
 
     osdHudWrite(poi_x, poi_y, poiSymbol, 1);
 
-    // Signal on the right, heading on the left
-
-    if (poiSignal < 5) { // 0 to 4 = signal bars, 5 = No LQ and no heading displayed
-        error_x = hudWrap360(poiHeading - DECIDEGREES_TO_DEGREES(osdGetHeading()));
+    if (poiType == 1) { // POI from the ESP radar
+        error_x = hudWrap360(poiP1 - DECIDEGREES_TO_DEGREES(osdGetHeading()));
         osdHudWrite(poi_x - 1, poi_y, SYM_DIRECTION + ((error_x + 22) / 45) % 8, 1);
-        osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiSignal, 1);
+        osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiP2, 1);
     }
+    else if (poiType == 2) { // Waypoint,
+        osdHudWrite(poi_x - 1, poi_y, SYM_HUD_ARROWS_U1 + poiP2, 1);
+        osdHudWrite(poi_x + 1, poi_y, poiP1, 1);
+        }
 
     // Distance
 
-    char buff[3];
     if ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) {
         osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3);
     }
diff --git a/src/main/io/osd_hud.h b/src/main/io/osd_hud.h
index 9744ad0ec4..3845c2347b 100644
--- a/src/main/io/osd_hud.h
+++ b/src/main/io/osd_hud.h
@@ -23,7 +23,7 @@
 void osdHudClear(void);
 void osdHudDrawCrosshair(uint8_t px, uint8_t py);
 void osdHudDrawHoming(uint8_t px, uint8_t py);
-void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol);
+void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2);
 void osdHudDrawExtras(uint8_t poi_id);
 int8_t radarGetNearestPOI(void);
 
diff --git a/src/main/io/pwmdriver_i2c.c b/src/main/io/pwmdriver_i2c.c
index 727b8428f9..dc5e930457 100644
--- a/src/main/io/pwmdriver_i2c.c
+++ b/src/main/io/pwmdriver_i2c.c
@@ -50,6 +50,10 @@ void pwmDriverInitialize(void) {
 }
 
 void pwmDriverSync(void) {
+    if (!STATE(PWM_DRIVER_AVAILABLE)) {
+        return;
+    }
+
     static uint8_t cycle = 0;
 
     (pwmDrivers[driverImplementationIndex].syncFunction)(cycle);
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index bea6abb001..6f77480f73 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -53,6 +53,7 @@ typedef enum {
     FUNCTION_TELEMETRY_SIM       = (1 << 19), // 524288
     FUNCTION_FRSKY_OSD           = (1 << 20), // 1048576
     FUNCTION_DJI_HD_OSD          = (1 << 21), // 2097152
+    FUNCTION_SERVO_SERIAL        = (1 << 22), // 4194304
 } serialPortFunction_e;
 
 typedef enum {
diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c
index 95ef85a465..7cf24890b7 100644
--- a/src/main/io/serial_4way.c
+++ b/src/main/io/serial_4way.c
@@ -72,7 +72,7 @@
 // *** change to adapt Revision
 #define SERIAL_4WAY_VER_MAIN 20
 #define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0
-#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 02
+#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03
 
 #define SERIAL_4WAY_PROTOCOL_VER 107
 // *** end
@@ -332,7 +332,8 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) {
         (pDeviceInfo->words[0] == 0xE8B2))
 
 #define ARM_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x1F06) || \
-        (pDeviceInfo->words[0] == 0x3306) || (pDeviceInfo->words[0] == 0x3406) || (pDeviceInfo->words[0] == 0x3506))
+        (pDeviceInfo->words[0] == 0x3306) || (pDeviceInfo->words[0] == 0x3406) || (pDeviceInfo->words[0] == 0x3506) || \
+        (pDeviceInfo->words[0] == 0x2B06) || (pDeviceInfo->words[0] == 0x4706))
 
 static uint8_t CurrentInterfaceMode;
 
diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c
index 3b711a1c55..5823006870 100644
--- a/src/main/io/serial_4way_avrootloader.c
+++ b/src/main/io/serial_4way_avrootloader.c
@@ -311,7 +311,7 @@ uint8_t BL_PageErase(ioMem_t *pMem)
     if (BL_SendCMDSetAddress(pMem)) {
         uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
         BL_SendBuf(sCMD, 2);
-        return (BL_GetACK((1400 / START_BIT_TIMEOUT_MS)) == brSUCCESS);
+        return (BL_GetACK((3000 / START_BIT_TIMEOUT_MS)) == brSUCCESS);
     }
     return 0;
 }
diff --git a/src/main/io/servo_sbus.c b/src/main/io/servo_sbus.c
new file mode 100644
index 0000000000..ba0edcc532
--- /dev/null
+++ b/src/main/io/servo_sbus.c
@@ -0,0 +1,133 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <ctype.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/crc.h"
+
+#include "io/serial.h"
+#include "io/servo_sbus.h"
+#include "rx/sbus_channels.h"
+
+#if defined(USE_SERVO_SBUS)
+
+#define SERVO_SBUS_UART_BAUD            100000
+#define SERVO_SBUS_OPTIONS              (SBUS_PORT_OPTIONS | SERIAL_INVERTED | SERIAL_UNIDIR)
+
+static serialPort_t * servoSbusPort = NULL;
+static sbusFrame_t sbusFrame;
+
+bool sbusServoInitialize(void)
+{
+    serialPortConfig_t * portConfig;
+
+    // Avoid double initialization
+    if (servoSbusPort) {
+        return true;
+    }
+
+    portConfig = findSerialPortConfig(FUNCTION_SERVO_SERIAL);
+    if (!portConfig) {
+        return false;
+    }
+
+    servoSbusPort = openSerialPort(portConfig->identifier, FUNCTION_SERVO_SERIAL, NULL, NULL, SERVO_SBUS_UART_BAUD, MODE_TX, SERVO_SBUS_OPTIONS);
+    if (!servoSbusPort) {
+        return false;
+    }
+
+    // SBUS V1 magical values
+    sbusFrame.syncByte = 0x0F;
+    sbusFrame.channels.flags = 0;
+    sbusFrame.channels.chan0  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan1  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan2  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan3  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan4  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan5  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan6  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan7  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan8  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan9  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan10 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan11 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan12 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan13 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan14 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan15 = sbusEncodeChannelValue(1500);
+    sbusFrame.endByte = 0x00;
+
+    return true;
+}
+
+void sbusServoUpdate(uint8_t index, uint16_t value)
+{
+    switch(index) {
+        case 0: sbusFrame.channels.chan0 = sbusEncodeChannelValue(value); break;
+        case 1: sbusFrame.channels.chan1 = sbusEncodeChannelValue(value); break;
+        case 2: sbusFrame.channels.chan2 = sbusEncodeChannelValue(value); break;
+        case 3: sbusFrame.channels.chan3 = sbusEncodeChannelValue(value); break;
+        case 4: sbusFrame.channels.chan4 = sbusEncodeChannelValue(value); break;
+        case 5: sbusFrame.channels.chan5 = sbusEncodeChannelValue(value); break;
+        case 6: sbusFrame.channels.chan6 = sbusEncodeChannelValue(value); break;
+        case 7: sbusFrame.channels.chan7 = sbusEncodeChannelValue(value); break;
+        case 8: sbusFrame.channels.chan8 = sbusEncodeChannelValue(value); break;
+        case 9: sbusFrame.channels.chan9 = sbusEncodeChannelValue(value); break;
+        case 10: sbusFrame.channels.chan10 = sbusEncodeChannelValue(value); break;
+        case 11: sbusFrame.channels.chan11 = sbusEncodeChannelValue(value); break;
+        case 12: sbusFrame.channels.chan12 = sbusEncodeChannelValue(value); break;
+        case 13: sbusFrame.channels.chan13 = sbusEncodeChannelValue(value); break;
+        case 14: sbusFrame.channels.chan14 = sbusEncodeChannelValue(value); break;
+        case 15: sbusFrame.channels.chan15 = sbusEncodeChannelValue(value); break;
+        default:
+            break;
+    }
+}
+
+void sbusServoSendUpdate(void)
+{
+    // Check if the port is initialized
+    if (!servoSbusPort) {
+        return;
+    }
+
+    // Skip update if previous one is not yet fully sent
+    // This helps to avoid buffer overflow and evenyually the data corruption
+    if (!isSerialTransmitBufferEmpty(servoSbusPort)) {
+        return;
+    }
+
+    serialWriteBuf(servoSbusPort, (const uint8_t *)&sbusFrame, sizeof(sbusFrame));
+}
+
+#endif
diff --git a/src/main/io/servo_sbus.h b/src/main/io/servo_sbus.h
new file mode 100644
index 0000000000..0dcc14ac8d
--- /dev/null
+++ b/src/main/io/servo_sbus.h
@@ -0,0 +1,29 @@
+/*
+ * 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
+
+bool sbusServoInitialize(void);
+void sbusServoUpdate(uint8_t index, uint16_t value);
+void sbusServoSendUpdate(void);
diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c
index 485a880353..972162264c 100644
--- a/src/main/io/vtx.c
+++ b/src/main/io/vtx.c
@@ -25,6 +25,7 @@
 
 #include "common/maths.h"
 #include "common/time.h"
+#include "common/utils.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@@ -56,7 +57,6 @@ typedef enum {
     VTX_PARAM_POWER = 0,
     VTX_PARAM_BANDCHAN,
     VTX_PARAM_PITMODE,
-    VTX_PARAM_CONFIRM,
     VTX_PARAM_COUNT
 } vtxScheduleParams_e;
 
@@ -91,7 +91,7 @@ static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice, const vtxSettingsCo
         return false;
     }
 
-    if(!ARMING_FLAG(ARMED)) {
+    if(!ARMING_FLAG(ARMED) && runtimeSettings->band) {
         uint8_t vtxBand;
         uint8_t vtxChan;
         if (!vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
@@ -122,6 +122,8 @@ static bool vtxProcessPower(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t *
 
 static bool vtxProcessPitMode(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
 {
+    UNUSED(runtimeSettings);
+
     uint8_t pitOnOff;
 
     bool        currPmSwitchState = false;
@@ -149,20 +151,6 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t
     return false;
 }
 
-static bool vtxProcessCheckParameters(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
-{
-    uint8_t vtxBand;
-    uint8_t vtxChan;
-    uint8_t vtxPower;
-
-    vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
-    vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan);
-
-    return (runtimeSettings->band && runtimeSettings->band != vtxBand) ||
-           (runtimeSettings->channel != vtxChan) ||
-           (runtimeSettings->power != vtxPower);
-}
-
 void vtxUpdate(timeUs_t currentTimeUs)
 {
     static uint8_t currentSchedule = 0;
@@ -191,9 +179,6 @@ void vtxUpdate(timeUs_t currentTimeUs)
             case VTX_PARAM_PITMODE:
                 vtxUpdatePending = vtxProcessPitMode(vtxDevice, runtimeSettings);
                 break;
-            case VTX_PARAM_CONFIRM:
-                vtxUpdatePending = vtxProcessCheckParameters(vtxDevice, runtimeSettings);
-                break;
             default:
                 break;
         }
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index a23d590201..cb60d447ad 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -24,6 +24,7 @@
 #include <stdint.h>
 #include <string.h>
 #include <ctype.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -37,6 +38,7 @@
 #include "common/maths.h"
 #include "common/printf.h"
 #include "common/utils.h"
+#include "common/typeconversion.h"
 
 #include "drivers/time.h"
 #include "drivers/vtx_common.h"
@@ -56,19 +58,23 @@
 
 static serialPort_t *smartAudioSerialPort = NULL;
 
-const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = {
-    "---", "25 ", "200", "500", "800",
+uint8_t saPowerCount = VTX_SMARTAUDIO_DEFAULT_POWER_COUNT;
+const char * saPowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT + 1] = {
+    "----", "25  ", "200 ", "500 ", "800 ", "    "
 };
 
+// Save powerlevels reported from SA 2.1 devices here
+char sa21PowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT][5];
+
 static const vtxVTable_t saVTable;    // Forward
 static vtxDevice_t vtxSmartAudio = {
     .vTable = &saVTable,
     .capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT,
     .capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT,
-    .capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT,
+    .capability.powerCount = VTX_SMARTAUDIO_MAX_POWER_COUNT,
     .capability.bandNames = (char **)vtx58BandNames,
     .capability.channelNames = (char **)vtx58ChannelNames,
-    .capability.powerNames = (char **)saPowerNames,
+    .capability.powerNames = (char**)saPowerNames
 };
 
 // SmartAudio command and response codes
@@ -110,22 +116,25 @@ smartAudioStat_t saStat = {
     .badcode = 0,
 };
 
-saPowerTable_t saPowerTable[VTX_SMARTAUDIO_POWER_COUNT] = {
-    {  25,   7,   0 },
-    { 200,  16,   1 },
-    { 500,  25,   2 },
-    { 800,  40,   3 },
+// Fill table with standard values for SA 1.0 and 2.0
+saPowerTable_t saPowerTable[VTX_SMARTAUDIO_MAX_POWER_COUNT] = {
+    {  25,   7 },
+    { 200,  16 },
+    { 500,  25 },
+    { 800,  40 },
+    {   0,   0 } // Placeholder
 };
 
 // Last received device ('hard') states
 
 smartAudioDevice_t saDevice = {
-    .version = 0,
+    .version = SA_UNKNOWN,
     .channel = -1,
     .power = -1,
     .mode = 0,
     .freq = 0,
     .orfreq = 0,
+    .willBootIntoPitMode = false
 };
 
 static smartAudioDevice_t saDevicePrev = {
@@ -139,7 +148,7 @@ static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable?
 bool saDeferred = true; // saCms variable?
 
 // Receive frame reassembly buffer
-#define SA_MAX_RCVLEN 15
+#define SA_MAX_RCVLEN 21
 static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard
 
 //
@@ -183,18 +192,31 @@ static void saPrintSettings(void)
     LOG_D(VTX, "freq: %d ", saDevice.freq);
     LOG_D(VTX, "power: %d ", saDevice.power);
     LOG_D(VTX, "pitfreq: %d ", saDevice.orfreq);
+    LOG_D(VTX, "BootIntoPitMode: %s", saDevice.willBootIntoPitMode ? "yes" : "no");
 }
 
 int saDacToPowerIndex(int dac)
 {
-    for (int idx = VTX_SMARTAUDIO_POWER_COUNT - 1 ; idx >= 0 ; idx--) {
-        if (saPowerTable[idx].valueV1 <= dac) {
+    for (int idx = saPowerCount - 1 ; idx >= 0 ; idx--) {
+        if (saPowerTable[idx].dbi <= dac) {
             return idx;
         }
     }
     return 0;
 }
 
+int saDbiToMw(uint16_t dbi) {
+
+    uint16_t mw = (uint16_t)pow(10.0, dbi / 10.0);
+
+    if (dbi > 14) {
+        // For powers greater than 25mW round up to a multiple of 50 to match expectations
+        mw = 50 * ((mw + 25) / 50);
+     }
+
+    return mw;
+}
+
 //
 // Autobauding
 //
@@ -272,14 +294,67 @@ static void saProcessResponse(uint8_t *buf, int len)
             break;
         }
 
-        // XXX(fujin): For now handle V21 saDevice.version as '2'.
         // From spec: "Bit 7-3 is holding the Smart audio version where 0 is V1, 1 is V2, 2 is V2.1"
-        saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
+        // saDevice.version = 0 means unknown, 1 means Smart audio V1, 2 means Smart audio V2 and 3 means Smart audio V2.1
+        saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : ((buf[0] == SA_CMD_GET_SETTINGS_V2) ? 2 : 3);
         saDevice.channel = buf[2];
-        saDevice.power = buf[3];
+        uint8_t rawPowerValue = buf[3];
         saDevice.mode = buf[4];
-        saDevice.freq = (buf[5] << 8)|buf[6];
-        // XXX(fujin): Receive additional SA2.1 fields here for dBm based power level.
+        saDevice.freq = (buf[5] << 8) | buf[6];
+
+        // read pir and por flags to detect if the device will boot into pitmode.
+        // note that "quit pitmode without unsetting the pitmode flag" clears pir and por flags but the device will still boot into pitmode.
+        // therefore we ignore the pir and por flags while the device is not in pitmode
+        // actually, this is the whole reason the variable saDevice.willBootIntoPitMode exists.
+        // otherwise we could use saDevice.mode directly
+        if (saDevice.mode & SA_MODE_GET_PITMODE) {
+            bool newBootMode = (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE);
+            if (newBootMode != saDevice.willBootIntoPitMode) {
+                LOG_D(VTX, "saProcessResponse: willBootIntoPitMode is now %s\r\n", newBootMode  ? "true" : "false");
+            }
+            saDevice.willBootIntoPitMode = newBootMode;
+        }
+
+        if(saDevice.version == SA_2_1) {
+            //read dbm based power levels
+            if(len < 10) { //current power level in dbm field missing or power level length field missing or zero power levels reported
+                LOG_D(VTX, "processResponse: V2.1 vtx didn't report any power levels\r\n");
+                break;
+            }
+            saPowerCount = constrain((int8_t)buf[8], 0, VTX_SMARTAUDIO_MAX_POWER_COUNT);
+            vtxSmartAudio.capability.powerCount = saPowerCount;
+            //SmartAudio seems to report buf[8] + 1 power levels, but one of them is zero.
+            //zero is indeed a valid power level to set the vtx to, but it activates pit mode.
+            //crucially, after sending 0 dbm, the vtx does NOT report its power level to be 0 dbm.
+            //instead, it reports whatever value was set previously and it reports to be in pit mode.
+            //for this reason, zero shouldn't be used as a normal power level in iNav.
+            for (int8_t i = 0; i < saPowerCount; i++ ) {
+                saPowerTable[i].dbi = buf[9 + i + 1]; //+ 1 to skip the first power level, as mentioned above
+                saPowerTable[i].mW = saDbiToMw(saPowerTable[i].dbi);
+                if (i <= VTX_SMARTAUDIO_MAX_POWER_COUNT) {
+                    char strbuf[5];
+                    itoa(saPowerTable[i].mW, strbuf, 10);
+                    strcpy(sa21PowerNames[i], strbuf);
+                    saPowerNames[i + 1] = sa21PowerNames[i];
+                }
+            }
+
+            LOG_D(VTX, "processResponse: %d power values: %d, %d, %d, %d\r\n",
+                    saPowerCount, saPowerTable[0].dbi, saPowerTable[1].dbi,
+                    saPowerTable[2].dbi, saPowerTable[3].dbi);
+            //LOG_D(VTX, "processResponse: V2.1 received vtx power value %d\r\n",buf[7]);
+            rawPowerValue = buf[7];
+
+            saDevice.power = 0; //set to unknown power level if the reported one doesnt match any of the known ones
+            LOG_D(VTX, "processResponse: rawPowerValue is %d, legacy power is %d\r\n", rawPowerValue, buf[3]);
+            for (int8_t i = 0; i < saPowerCount; i++) {
+                if (rawPowerValue == saPowerTable[i].dbi) {
+                    saDevice.power = i + 1;
+                }
+            }
+        } else {
+            saDevice.power = rawPowerValue + 1;
+        }
 
         DEBUG_SET(DEBUG_SMARTAUDIO, 0, saDevice.version * 100 + saDevice.mode);
         DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel);
@@ -288,14 +363,6 @@ static void saProcessResponse(uint8_t *buf, int len)
         break;
 
     case SA_CMD_SET_POWER: // Set Power
-        if (len == 5) {
-            // Improperly implemented S.Audio devices that
-            // omit the channel
-            saDevice.power = buf[2];
-        } else if (len >= 6) {
-            // S.Audio v2 spec
-            saDevice.power = buf[3];
-        }
         break;
 
     case SA_CMD_SET_CHAN: // Set Channel
@@ -321,7 +388,9 @@ static void saProcessResponse(uint8_t *buf, int len)
         break;
 
     case SA_CMD_SET_MODE: // Set Mode
-        LOG_D(VTX, "saProcessResponse: SET_MODE 0x%x", buf[2]);
+        LOG_D(VTX, "saProcessResponse: SET_MODE 0x%x, (pir %s, por %s, pitdsbl %s, %s)\r\n",
+            buf[2], (buf[2] & 1) ? "on" : "off", (buf[2] & 2) ? "on" : "off", (buf[3] & 4) ? "on" : "off",
+            (buf[4] & 8) ? "unlocked" : "locked");
         break;
 
     default:
@@ -330,18 +399,11 @@ static void saProcessResponse(uint8_t *buf, int len)
     }
 
     if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
-#ifdef USE_CMS    //if changes then trigger saCms update
-        //saCmsResetOpmodel();
-#endif
-    // Debug
-    saPrintSettings();
+        // Debug
+        saPrintSettings();
     }
-    saDevicePrev = saDevice;
 
-#ifdef USE_CMS
-    // Export current device status for CMS
-    //saCmsUpdate();
-#endif
+    saDevicePrev = saDevice;
 }
 
 //
@@ -536,10 +598,11 @@ static void saGetSettings(void)
 {
     static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F};
 
+    LOG_D(VTX, "smartAudioGetSettings\r\n");
     saQueueCmd(bufGetSettings, 5);
 }
 
-static void saDoDevSetFreq(uint16_t freq)
+void saSetFreq(uint16_t freq)
 {
     static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
     static uint8_t switchBuf[7];
@@ -572,14 +635,9 @@ static void saDoDevSetFreq(uint16_t freq)
     saQueueCmd(buf, 7);
 }
 
-void saSetFreq(uint16_t freq)
-{
-    saDoDevSetFreq(freq);
-}
-
 void saSetPitFreq(uint16_t freq)
 {
-    saDoDevSetFreq(freq | SA_FREQ_SETPIT);
+    saSetFreq(freq | SA_FREQ_SETPIT);
 }
 
 #if 0
@@ -592,58 +650,35 @@ static void saGetPitFreq(void)
 static bool saValidateBandAndChannel(uint8_t band, uint8_t channel)
 {
     return (band >= VTX_SMARTAUDIO_MIN_BAND && band <= VTX_SMARTAUDIO_MAX_BAND &&
-             channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= VTX_SMARTAUDIO_MAX_CHANNEL);
+            channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= VTX_SMARTAUDIO_MAX_CHANNEL);
 }
 
-static void saDevSetBandAndChannel(uint8_t band, uint8_t channel)
+void saSetBandAndChannel(uint8_t band, uint8_t channel)
 {
     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };
 
     buf[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel);
     buf[5] = CRC8(buf, 5);
+    LOG_D(VTX, "vtxSASetBandAndChannel set index band %d channel %d value sent 0x%x\r\n", band, channel, buf[4]);
 
+    //this will clear saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ
     saQueueCmd(buf, 6);
 }
 
-void saSetBandAndChannel(uint8_t band, uint8_t channel)
-{
-    saDevSetBandAndChannel(band, channel);
-}
-
 void saSetMode(int mode)
 {
     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
 
     buf[4] = (mode & 0x3f) | saLockMode;
-    buf[5] = CRC8(buf, 5);
-
-    saQueueCmd(buf, 6);
-}
-
-static void saDevSetPowerByIndex(uint8_t index)
-{
-    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
-
-    LOG_D(VTX, "saSetPowerByIndex: index %d", index);
-
-    if (index == 0) {
-        // SmartAudio doesn't support power off.
-        return;
+    if (saDevice.version >= SA_2_1 && (mode & SA_MODE_CLR_PITMODE) &&
+        ((mode & SA_MODE_SET_IN_RANGE_PITMODE) || (mode & SA_MODE_SET_OUT_RANGE_PITMODE))) {
+        saDevice.willBootIntoPitMode = true;//quit pitmode without unsetting flag.
+        //the response will just say pit=off but the device will still go into pitmode on reboot.
+        //therefore we have to memorize this change here.
     }
+    LOG_D(VTX, "saSetMode(0x%x): pir=%s por=%s pitdsbl=%s %s\r\n", mode, (mode & 1) ? "on " : "off", (mode & 2) ? "on " : "off",
+            (mode & 4)? "on " : "off", (mode & 8) ? "locked" : "unlocked");
 
-    if (index > VTX_SMARTAUDIO_POWER_COUNT) {
-        // Invalid power level
-        return;
-    }
-
-    if (saDevice.version == 0) {
-        // Unknown or yet unknown version.
-        return;
-    }
-
-    unsigned entry = index - 1;
-
-    buf[4] = (saDevice.version == 1) ? saPowerTable[entry].valueV1 : saPowerTable[entry].valueV2;
     buf[5] = CRC8(buf, 5);
     saQueueCmd(buf, 6);
 }
@@ -701,12 +736,17 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
         // Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ,
         // and put the device into user frequency mode with uninitialized freq.
         if (saDevice.version) {
-            if (saDevice.version == 2) {
-                saDoDevSetFreq(SA_FREQ_GETPIT);
+            if (saDevice.version == SA_2_0) {
+                saSetFreq(SA_FREQ_GETPIT);
                 initPhase = SA_INITPHASE_WAIT_PITFREQ;
             } else {
                 initPhase = SA_INITPHASE_DONE;
             }
+            if (saDevice.version >= SA_2_0 ) {
+                //did the device boot up in pit mode on its own?
+                saDevice.willBootIntoPitMode = (saDevice.mode & SA_MODE_GET_PITMODE) ? true : false;
+                LOG_D(VTX, "sainit: willBootIntoPitMode is %s\r\n", saDevice.willBootIntoPitMode ? "true" : "false");
+            }
         }
         break;
 
@@ -735,7 +775,7 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
         // Command pending. Send it.
         // LOG_D(VTX, "process: sending queue");
         saSendQueue();
-    lastCommandSentMs = nowMs;
+        lastCommandSentMs = nowMs;
     } else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW) && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) {
     //LOG_D(VTX, "process: sending status change polling");
     saGetSettings();
@@ -753,45 +793,100 @@ vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice)
 
 static bool vtxSAIsReady(const vtxDevice_t *vtxDevice)
 {
-    return vtxDevice!=NULL && !(saDevice.version == 0);
+    return vtxDevice != NULL && !(saDevice.power == 0);
+    //wait until power reading exists
 }
 
-static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
+void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 {
     UNUSED(vtxDevice);
     if (saValidateBandAndChannel(band, channel)) {
         saSetBandAndChannel(band - 1, channel - 1);
     }
 }
-
-static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
+ static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 {
-    UNUSED(vtxDevice);
-    saDevSetPowerByIndex(index);
+    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
+
+    if (!vtxSAIsReady(vtxDevice)) {
+        return;
+    }
+
+    if (index == 0) {
+        LOG_D(VTX, "SmartAudio doesn't support power off");
+        return;
+    }
+
+    if (index > saPowerCount) {
+        LOG_D(VTX, "Invalid power level");
+        return;
+    }
+
+    LOG_D(VTX, "saSetPowerByIndex: index %d, value %d\r\n", index, buf[4]);
+
+    index--;
+    switch (saDevice.version) {
+        case SA_1_0:
+            buf[4] = saPowerTable[index].dbi;
+            break;
+        case SA_2_0:
+            buf[4] = index;
+            break;
+        case SA_2_1:
+            buf[4] = saPowerTable[index].dbi;
+            buf[4] |= 128; //set MSB to indicate set power by dbm
+            break;
+        default:
+            break;
+    }
+
+    buf[5] = CRC8(buf, 5);
+    saQueueCmd(buf, 6);
 }
 
 static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 {
-    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
+    if (!vtxSAIsReady(vtxDevice) || saDevice.version < SA_1_0) {
         return;
     }
 
-    if (onoff) {
-        // SmartAudio can not turn pit mode on by software.
+    if (onoff && saDevice.version < SA_2_1) {
+        // Smart Audio prior to V2.1 can not turn pit mode on by software.
         return;
     }
 
-    uint8_t newmode = SA_MODE_CLR_PITMODE;
-
-    if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
-        newmode |= SA_MODE_SET_IN_RANGE_PITMODE;
+    if (saDevice.version >= SA_2_1 && !saDevice.willBootIntoPitMode) {
+        if (onoff) {
+            // enable pitmode using SET_POWER command with 0 dbm.
+            // This enables pitmode without causing the device to boot into pitmode next power-up
+            static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
+            buf[4] = 0 | 128;
+            buf[5] = CRC8(buf, 5);
+            saQueueCmd(buf, 6);
+            LOG_D(VTX, "vtxSASetPitMode: set power to 0 dbm\r\n");
+        } else {
+            saSetMode(SA_MODE_CLR_PITMODE);
+            LOG_D(VTX, "vtxSASetPitMode: clear pitmode permanently");
+        }
+       return;
     }
 
+    uint8_t newMode = onoff ? 0 : SA_MODE_CLR_PITMODE;
+
     if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) {
-        newmode |= SA_MODE_SET_OUT_RANGE_PITMODE;
+        newMode |= SA_MODE_SET_OUT_RANGE_PITMODE;
     }
 
-    saSetMode(newmode);
+    if ((saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (onoff && newMode == 0)) {
+        // ensure when turning on pit mode that pit mode gets actually enabled
+        newMode |= SA_MODE_SET_IN_RANGE_PITMODE;
+    }
+    LOG_D(VTX, "vtxSASetPitMode %s with stored mode 0x%x por %s, pir %s, newMode 0x%x\r\n", onoff ? "on" : "off", saDevice.mode,
+            (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) ? "on" : "off",
+            (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) ? "on" : "off" , newMode);
+
+
+    saSetMode(newMode);
 
     return;
 }
@@ -806,6 +901,7 @@ static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand,
     *pBand = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 0 :
         (SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1);
     *pChannel = SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1;
+
     return true;
 }
 
@@ -815,13 +911,13 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
         return false;
     }
 
-    *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
+    *pIndex = ((saDevice.version == SA_1_0) ? saDacToPowerIndex(saDevice.power) : saDevice.power);
     return true;
 }
 
 static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 {
-    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
+    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version < SA_2_0))) {
         return false;
     }
 
@@ -851,7 +947,7 @@ static bool vtxSAGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_
     }
 
     *pIndex = powerIndex;
-    *pPowerMw = (powerIndex > 0) ? saPowerTable[powerIndex-1].rfpower : 0;
+    *pPowerMw = (powerIndex > 0) ? saPowerTable[powerIndex - 1].mW : 0;
     return true;
 }
 
diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h
index ef3e147ace..c817f05a79 100644
--- a/src/main/io/vtx_smartaudio.h
+++ b/src/main/io/vtx_smartaudio.h
@@ -33,7 +33,8 @@
 #define VTX_SMARTAUDIO_BAND_COUNT (VTX_SMARTAUDIO_MAX_BAND - VTX_SMARTAUDIO_MIN_BAND + 1)
 #define VTX_SMARTAUDIO_CHANNEL_COUNT (VTX_SMARTAUDIO_MAX_CHANNEL - VTX_SMARTAUDIO_MIN_CHANNEL + 1)
 
-#define VTX_SMARTAUDIO_POWER_COUNT 4
+#define VTX_SMARTAUDIO_MAX_POWER_COUNT 5
+#define VTX_SMARTAUDIO_DEFAULT_POWER_COUNT 4
 #define VTX_SMARTAUDIO_DEFAULT_POWER 1
 
 #define VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ 5000        //min freq in MHz
@@ -49,7 +50,7 @@
 
 // opmode flags, SET side
 #define SA_MODE_SET_IN_RANGE_PITMODE        1 // Immediate
-#define SA_MODE_SET_OUT_RANGE_PITMODE        2 // Immediate
+#define SA_MODE_SET_OUT_RANGE_PITMODE       2 // Immediate
 #define SA_MODE_CLR_PITMODE                 4 // Immediate
 #define SA_MODE_SET_UNLOCK                  8
 #define SA_MODE_SET_LOCK                    0 // ~UNLOCK
@@ -61,20 +62,26 @@
 #define SA_FREQ_MASK                        (~(SA_FREQ_GETPIT|SA_FREQ_SETPIT))
 
 // For generic API use, but here for now
+typedef enum {
+    SA_UNKNOWN, // or offline
+    SA_1_0,
+    SA_2_0,
+    SA_2_1
+} smartAudioVersion_e;
 
 typedef struct smartAudioDevice_s {
-    int8_t version;
+    smartAudioVersion_e version;
     int8_t channel;
     int8_t power;
     int8_t mode;
     uint16_t freq;
     uint16_t orfreq;
+    bool willBootIntoPitMode;
 } smartAudioDevice_t;
 
 typedef struct saPowerTable_s {
-    int rfpower;
-    int16_t valueV1;
-    int16_t valueV2;
+    int mW;         // rfpower
+    int16_t dbi;    // valueV1
 } saPowerTable_t;
 
 typedef struct smartAudioStat_s {
@@ -89,7 +96,9 @@ typedef struct smartAudioStat_s {
 
 extern smartAudioDevice_t saDevice;
 extern saPowerTable_t saPowerTable[];
-extern const char * const saPowerNames[];
+
+extern uint8_t saPowerCount;
+extern const char * saPowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT + 1];
 extern smartAudioStat_t saStat;
 
 extern uint16_t sa_smartbaud;
diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c
index 14f0811b31..3e95ac1ae8 100644
--- a/src/main/io/vtx_tramp.c
+++ b/src/main/io/vtx_tramp.c
@@ -30,11 +30,12 @@
 #if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL)
 
 #include "build/debug.h"
+#include "drivers/vtx_common.h"
+#include "drivers/time.h"
 
 #include "common/maths.h"
 #include "common/utils.h"
-
-#include "drivers/vtx_common.h"
+#include "common/crc.h"
 
 #include "io/serial.h"
 #include "io/vtx_tramp.h"
@@ -42,565 +43,556 @@
 #include "io/vtx.h"
 #include "io/vtx_string.h"
 
-const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
-    25, 100, 200, 400, 600
-};
+#define VTX_PKT_SIZE                16
+#define VTX_PROTO_STATE_TIMEOUT_MS  1000
+#define VTX_STATUS_INTERVAL_MS      2000
 
-const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = {
-    "---", "25 ", "100", "200", "400", "600"
-};
-
-static const vtxVTable_t trampVTable; // forward
-static vtxDevice_t vtxTramp = {
-    .vTable = &trampVTable,
-    .capability.bandCount = VTX_TRAMP_BAND_COUNT,
-    .capability.channelCount = VTX_TRAMP_CHANNEL_COUNT,
-    .capability.powerCount = VTX_TRAMP_POWER_COUNT,
-    .capability.bandNames = (char **)vtx58BandNames,
-    .capability.channelNames = (char **)vtx58ChannelNames,
-    .capability.powerNames = (char **)trampPowerNames,
-};
-
-static serialPort_t *trampSerialPort = NULL;
-
-static uint8_t trampRespBuffer[16];
+#define VTX_UPDATE_REQ_NONE         0x00
+#define VTX_UPDATE_REQ_FREQUENCY    0x01
+#define VTX_UPDATE_REQ_POWER        0x02
+#define VTX_UPDATE_REQ_PITMODE      0x04
 
 typedef enum {
-    TRAMP_STATUS_BAD_DEVICE = -1,
-    TRAMP_STATUS_OFFLINE = 0,
-    TRAMP_STATUS_ONLINE,
-    TRAMP_STATUS_SET_FREQ_PW,
-    TRAMP_STATUS_CHECK_FREQ_PW
-} trampStatus_e;
-
-trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
-
-uint32_t trampRFFreqMin;
-uint32_t trampRFFreqMax;
-uint32_t trampRFPowerMax;
-
-trampData_t trampData;
-
-// Maximum number of requests sent to try a config change
-#define TRAMP_MAX_RETRIES 2
-
-uint32_t trampConfFreq = 0;
-uint8_t  trampFreqRetries = 0;
-
-uint16_t trampConfPower = 0;
-uint8_t  trampPowerRetries = 0;
-
-static void trampWriteBuf(uint8_t *buf)
-{
-    serialWriteBuf(trampSerialPort, buf, 16);
-}
-
-static uint8_t trampChecksum(uint8_t *trampBuf)
-{
-    uint8_t cksum = 0;
-
-    for (int i = 1 ; i < 14 ; i++) {
-        cksum += trampBuf[i];
-    }
-
-    return cksum;
-}
-
-void trampCmdU16(uint8_t cmd, uint16_t param)
-{
-    if (!trampSerialPort) {
-        return;
-    }
-
-    uint8_t trampReqBuffer[16];
-    memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
-    trampReqBuffer[0] = 15;
-    trampReqBuffer[1] = cmd;
-    trampReqBuffer[2] = param & 0xff;
-    trampReqBuffer[3] = (param >> 8) & 0xff;
-    trampReqBuffer[14] = trampChecksum(trampReqBuffer);
-    trampWriteBuf(trampReqBuffer);
-}
-
-static void trampDevSetFreq(uint16_t freq)
-{
-    trampConfFreq = freq;
-    if (trampConfFreq != trampData.curFreq) {
-        trampFreqRetries = TRAMP_MAX_RETRIES;
-    }
-}
-
-void trampSetFreq(uint16_t freq)
-{
-    trampData.setByFreqFlag = true;         //set freq via MHz value
-    trampDevSetFreq(freq);
-}
-
-void trampSendFreq(uint16_t freq)
-{
-    trampCmdU16('F', freq);
-}
-
-static bool trampValidateBandAndChannel(uint8_t band, uint8_t channel)
-{
-    return (band >= VTX_TRAMP_MIN_BAND && band <= VTX_TRAMP_MAX_BAND &&
-            channel >= VTX_TRAMP_MIN_CHANNEL && channel <= VTX_TRAMP_MAX_CHANNEL);
-}
-
-static void trampDevSetBandAndChannel(uint8_t band, uint8_t channel)
-{
-    trampDevSetFreq(vtx58_Bandchan2Freq(band, channel));
-}
-
-void trampSetBandAndChannel(uint8_t band, uint8_t channel)
-{
-    trampData.setByFreqFlag = false;        //set freq via band/channel
-    trampDevSetBandAndChannel(band, channel);
-}
-
-void trampSetRFPower(uint16_t level)
-{
-    trampConfPower = level;
-    if (trampConfPower != trampData.power) {
-        trampPowerRetries = TRAMP_MAX_RETRIES;
-    }
-}
-
-void trampSendRFPower(uint16_t level)
-{
-    trampCmdU16('P', level);
-}
-
-// return false if error
-bool trampCommitChanges(void)
-{
-    if (trampStatus != TRAMP_STATUS_ONLINE) {
-        return false;
-    }
-
-    trampStatus = TRAMP_STATUS_SET_FREQ_PW;
-    return true;
-}
-
-// return false if index out of range
-static bool trampDevSetPowerByIndex(uint8_t index)
-{
-    if (index > 0 && index <= VTX_TRAMP_POWER_COUNT) {
-        trampSetRFPower(trampPowerTable[index - 1]);
-        trampCommitChanges();
-        return true;
-    }
-    return false;
-}
-
-void trampSetPitMode(uint8_t onoff)
-{
-    trampCmdU16('I', onoff ? 0 : 1);
-}
-
-// returns completed response code
-char trampHandleResponse(void)
-{
-    const uint8_t respCode = trampRespBuffer[1];
-
-    switch (respCode) {
-    case 'r':
-        {
-            const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
-            if (min_freq != 0) {
-                trampRFFreqMin = min_freq;
-                trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
-                trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
-                return 'r';
-            }
-
-            // throw bytes echoed from tx to rx in bidirectional mode away
-        }
-        break;
-
-    case 'v':
-        {
-            const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
-            if (freq != 0) {
-                trampData.curFreq = freq;
-                trampData.configuredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
-                trampData.pitMode = trampRespBuffer[7];
-                trampData.power = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
-
-                // if no band/chan match then make sure set-by-freq mode is flagged
-                if (!vtx58_Freq2Bandchan(trampData.curFreq, &trampData.band, &trampData.channel)) {
-                    trampData.setByFreqFlag = true;
-                }
-
-                if (trampConfFreq == 0)  trampConfFreq  = trampData.curFreq;
-                if (trampConfPower == 0) trampConfPower = trampData.power;
-                return 'v';
-            }
-
-            // throw bytes echoed from tx to rx in bidirectional mode away
-        }
-        break;
-
-    case 's':
-        {
-            const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
-            if (temp != 0) {
-                trampData.temperature = temp;
-                return 's';
-            }
-        }
-        break;
-    }
-
-    return 0;
-}
+    VTX_STATE_RESET         = 0,
+    VTX_STATE_OFFILE        = 1,    // Not detected
+    VTX_STATE_DETECTING     = 2,    // 
+    VTX_STATE_IDLE          = 3,    // Idle, ready to sent commands
+    VTX_STATE_QUERY_DELAY   = 4,
+    VTX_STATE_QUERY_STATUS  = 5,
+    VTX_STATE_WAIT_STATUS   = 6,    // Wait for VTX state
+} vtxProtoState_e;
 
 typedef enum {
-    S_WAIT_LEN = 0,   // Waiting for a packet len
-    S_WAIT_CODE,      // Waiting for a response code
-    S_DATA,           // Waiting for rest of the packet.
-} trampReceiveState_e;
+    VTX_RESPONSE_TYPE_NONE,
+    VTX_RESPONSE_TYPE_CAPABILITIES,
+    VTX_RESPONSE_TYPE_STATUS,
+} vtxProtoResponseType_e;
 
-static trampReceiveState_e trampReceiveState = S_WAIT_LEN;
-static int trampReceivePos = 0;
+typedef struct {
+    vtxProtoState_e protoState;
+    timeMs_t        lastStateChangeMs;
+    timeMs_t        lastStatusQueryMs;
+    int             protoTimeoutCount;
+    unsigned        updateReqMask;
 
-static void trampResetReceiver(void)
-{
-    trampReceiveState = S_WAIT_LEN;
-    trampReceivePos = 0;
-}
+    // VTX capabilities
+    struct {
+        unsigned freqMin;   // min freq
+        unsigned freqMax;   // max freq
+        unsigned powerMax;  //
+    } capabilities;
+
+    // Requested VTX state
+    struct {
+        // Only tracking
+        int      band;
+        int      channel;
+        unsigned powerIndex;
+
+        // Actual settings to send to the VTX
+        unsigned freq;
+        unsigned power;
+    } request;
+
+    // Actual VTX state: updated from actual VTX
+    struct {
+        unsigned freq;      // Frequency in MHz
+        unsigned power;
+        unsigned temp;
+        bool     pitMode;
+    } state;
+
+    struct {
+        int              powerTableCount;
+        const uint16_t * powerTablePtr;
+    } metadata;
+
+    // Comms flags and state
+    uint8_t         sendPkt[VTX_PKT_SIZE];
+    uint8_t         recvPkt[VTX_PKT_SIZE];
+    unsigned        recvPtr;
+    serialPort_t *  port;
+} vtxProtoState_t;
+
+static vtxProtoState_t vtxState;
+
+static void vtxProtoUpdatePowerMetadata(uint16_t maxPower);
 
 static bool trampIsValidResponseCode(uint8_t code)
 {
-    if (code == 'r' || code == 'v' || code == 's') {
-        return true;
-    } else {
-        return false;
-    }
+    return (code == 'r' || code == 'v' || code == 's');
 }
 
-// returns completed response code or 0
-static char trampReceive(uint32_t currentTimeUs)
+static bool vtxProtoRecv(void)
 {
+    uint8_t * bufPtr = (uint8_t*)&vtxState.recvPkt;
+    while (serialRxBytesWaiting(vtxState.port)) {
+        const uint8_t c = serialRead(vtxState.port);
+
+        if (vtxState.recvPtr == 0) {
+            // Wait for sync byte
+            if (c == 0x0F) {
+                bufPtr[vtxState.recvPtr++] = c;
+            }
+        }
+        else if (vtxState.recvPtr == 1) {
+            // Check if we received a valid response code
+            if (trampIsValidResponseCode(c)) {
+                bufPtr[vtxState.recvPtr++] = c;
+            }
+            else {
+                vtxState.recvPtr = 0;
+            }
+        }
+        else {
+            // Consume character and check if we have got a full packet
+            if (vtxState.recvPtr < VTX_PKT_SIZE) {
+                bufPtr[vtxState.recvPtr++] = c;
+            }
+
+            if (vtxState.recvPtr == VTX_PKT_SIZE) {
+                // Full packet received - validate packet, make sure it's the one we expect
+                const bool pktValid = ((bufPtr[14] == crc8_sum_update(0, &bufPtr[1], 13)) && (bufPtr[15] == 0));
+
+                if (!pktValid) {
+                    // Reset the receiver state - keep waiting
+                    vtxState.recvPtr = 0;
+                }
+                // Make sure it's not the echo one (half-duplex serial might receive it's own data)
+                else if (memcmp(&vtxState.recvPkt, &vtxState.sendPkt, VTX_PKT_SIZE) == 0) {
+                    vtxState.recvPtr = 0;
+                }
+                // Valid receive packet
+                else {
+                    return true;
+                }
+            }
+        }
+    }
+
+    return false;
+}
+
+static void vtxProtoSend(uint8_t cmd, uint16_t param)
+{
+    // Craft the packet
+    memset(vtxState.sendPkt, 0, ARRAYLEN(vtxState.sendPkt));
+    vtxState.sendPkt[0] = 15;
+    vtxState.sendPkt[1] = cmd;
+    vtxState.sendPkt[2] = param & 0xff;
+    vtxState.sendPkt[3] = (param >> 8) & 0xff;
+    vtxState.sendPkt[14] = crc8_sum_update(0, &vtxState.sendPkt[1], 13);
+
+    // Send data 
+    serialWriteBuf(vtxState.port, (uint8_t *)&vtxState.sendPkt, sizeof(vtxState.sendPkt));
+
+    // Reset cmd response state
+    vtxState.recvPtr = 0;
+}
+
+static void vtxProtoSetState(vtxProtoState_e newState)
+{
+    vtxState.lastStateChangeMs = millis();
+    vtxState.protoState = newState;
+}
+
+static bool vtxProtoTimeout(void)
+{
+    return (millis() - vtxState.lastStateChangeMs) > VTX_PROTO_STATE_TIMEOUT_MS;
+}
+
+static void vtxProtoQueryCapabilities(void)
+{
+    vtxProtoSend(0x72, 0);
+}
+
+static void vtxProtoQueryStatus(void)
+{
+    vtxProtoSend(0x76, 0);
+    vtxState.lastStatusQueryMs = millis();
+}
+
+/*
+static void vtxProtoQueryTemperature(void)
+{
+    vtxProtoSend('s', 0);
+}
+*/
+
+static vtxProtoResponseType_e vtxProtoProcessResponse(void)
+{
+    const uint8_t respCode = vtxState.recvPkt[1];
+
+    switch (respCode) {
+        case 0x72:
+            vtxState.capabilities.freqMin = vtxState.recvPkt[2] | (vtxState.recvPkt[3] << 8);
+            vtxState.capabilities.freqMax = vtxState.recvPkt[4] | (vtxState.recvPkt[5] << 8);
+            vtxState.capabilities.powerMax = vtxState.recvPkt[6] | (vtxState.recvPkt[7] << 8);
+            if (vtxState.capabilities.freqMin != 0 && vtxState.capabilities.freqMin < vtxState.capabilities.freqMax) {
+                // Update max power metadata so OSD settings would match VTX capabiolities
+                vtxProtoUpdatePowerMetadata(vtxState.capabilities.powerMax);
+                return VTX_RESPONSE_TYPE_CAPABILITIES;
+            }
+            break;
+
+        case 0x76:
+            vtxState.state.freq = vtxState.recvPkt[2] | (vtxState.recvPkt[3] << 8);
+            vtxState.state.power = vtxState.recvPkt[4]|(vtxState.recvPkt[5] << 8);
+            vtxState.state.pitMode = vtxState.recvPkt[7];
+            //vtxState.state.power = vtxState.recvPkt[8]|(vtxState.recvPkt[9] << 8);
+            return VTX_RESPONSE_TYPE_STATUS;
+    }
+
+    return VTX_RESPONSE_TYPE_NONE;
+}
+
+static void vtxProtoSetPitMode(uint16_t mode)
+{
+    vtxProtoSend(0x73, mode);
+}
+
+static void vtxProtoSetPower(uint16_t power)
+{
+    vtxProtoSend(0x50, power);
+}
+
+static void vtxProtoSetFrequency(uint16_t freq)
+{
+    vtxProtoSend(0x46, freq);
+}
+
+static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
+{
+    // Glue function betwen VTX VTable and actual driver protothread
+    UNUSED(vtxDevice);
     UNUSED(currentTimeUs);
 
-    if (!trampSerialPort) {
-        return 0;
+    if (!vtxState.port) {
+        return false;
     }
 
-    while (serialRxBytesWaiting(trampSerialPort)) {
-        const uint8_t c = serialRead(trampSerialPort);
-        trampRespBuffer[trampReceivePos++] = c;
+    switch((int)vtxState.protoState) {
+        case VTX_STATE_RESET:
+            vtxState.protoTimeoutCount = 0;
+            vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
+            vtxProtoSetState(VTX_STATE_OFFILE);
+            break;
 
-        switch (trampReceiveState) {
-        case S_WAIT_LEN:
-            if (c == 0x0F) {
-                trampReceiveState = S_WAIT_CODE;
-            } else {
-                trampReceivePos = 0;
+        // Send request for capabilities
+        case VTX_STATE_OFFILE:
+            vtxProtoQueryCapabilities();
+            vtxProtoSetState(VTX_STATE_DETECTING);
+            break;
+
+        // Detect VTX. We only accept VTX_RESPONSE_TYPE_CAPABILITIES here
+        case VTX_STATE_DETECTING:
+            if (vtxProtoRecv()) {
+                if (vtxProtoProcessResponse() == VTX_RESPONSE_TYPE_CAPABILITIES) {
+                    // VTX sent capabilities. Query status now
+                    vtxState.protoTimeoutCount = 0;
+                    vtxProtoSetState(VTX_STATE_QUERY_STATUS);
+                }
+                else {
+                    // Unexpected response. Re-initialize
+                    vtxProtoSetState(VTX_STATE_RESET);
+                }
+            }
+            else if (vtxProtoTimeout()) {
+                // Time-out while waiting for capabilities. Reset the state
+                vtxProtoSetState(VTX_STATE_RESET);
             }
             break;
 
-        case S_WAIT_CODE:
-            if (trampIsValidResponseCode(c)) {
-                trampReceiveState = S_DATA;
-            } else {
-                trampResetReceiver();
+        // Send requests to update freqnecy and power, periodically poll device for liveness
+        case VTX_STATE_IDLE:
+            if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) {
+                // Updates pending. Send an appropriate command
+                if (vtxState.updateReqMask & VTX_UPDATE_REQ_PITMODE) {
+                    // Only disabling PIT mode supported
+                    vtxState.updateReqMask &= ~VTX_UPDATE_REQ_PITMODE;
+                    vtxProtoSetPitMode(0);
+                    vtxProtoSetState(VTX_STATE_QUERY_DELAY);
+                }
+                else if (vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) {
+                    vtxState.updateReqMask &= ~VTX_UPDATE_REQ_FREQUENCY;
+                    vtxProtoSetFrequency(vtxState.request.freq);
+                    vtxProtoSetState(VTX_STATE_QUERY_DELAY);
+                }
+                else if (vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) {
+                    vtxState.updateReqMask &= ~VTX_UPDATE_REQ_POWER;
+                    vtxProtoSetPower(vtxState.request.power);
+                    vtxProtoSetState(VTX_STATE_QUERY_DELAY);
+                }
+            }
+            else if ((millis() - vtxState.lastStatusQueryMs) > VTX_STATUS_INTERVAL_MS) {
+                // Poll VTX for status updates
+                vtxProtoSetState(VTX_STATE_QUERY_STATUS);
             }
             break;
 
-        case S_DATA:
-            if (trampReceivePos == 16) {
-                uint8_t cksum = trampChecksum(trampRespBuffer);
+        case VTX_STATE_QUERY_DELAY:
+            // We get here after sending the command. We give VTX some time to process the command
+            // and switch to VTX_STATE_QUERY_STATUS
+            if (vtxProtoTimeout()) {
+                // We gave VTX some time to process the command. Query status to confirm success
+                vtxProtoSetState(VTX_STATE_QUERY_STATUS);
+            }
+            break;
 
-                trampResetReceiver();
+        case VTX_STATE_QUERY_STATUS:
+            // Just query status, nothing special
+            vtxProtoQueryStatus();
+            vtxProtoSetState(VTX_STATE_WAIT_STATUS);
+            break;
 
-                if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) {
-                    return trampHandleResponse();
+        case VTX_STATE_WAIT_STATUS:
+            if (vtxProtoRecv()) {
+                vtxState.protoTimeoutCount = 0;
+
+                if (vtxProtoProcessResponse() == VTX_RESPONSE_TYPE_STATUS) {
+                    // Check if VTX state matches VTX request
+                    if (!(vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) && (vtxState.state.freq != vtxState.request.freq)) {
+                        vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
+                    }
+
+                    if (!(vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) && (vtxState.state.power != vtxState.request.power)) {
+                        vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
+                    }
+
+                    // We got the status response - proceed to IDLE
+                    vtxProtoSetState(VTX_STATE_IDLE);
+                }
+                else {
+                    // Unexpected response. Query for STATUS again
+                    vtxProtoSetState(VTX_STATE_QUERY_STATUS);
+                }
+            }
+            else if (vtxProtoTimeout()) {
+                vtxState.protoTimeoutCount++;
+                if (vtxState.protoTimeoutCount > 3) {
+                    vtxProtoSetState(VTX_STATE_RESET);
+                }
+                else {
+                    vtxProtoSetState(VTX_STATE_QUERY_STATUS);
                 }
             }
             break;
-
-        default:
-            trampResetReceiver();
-            break;
-        }
     }
-
-    return 0;
 }
 
-void trampQuery(uint8_t cmd)
-{
-    trampResetReceiver();
-    trampCmdU16(cmd, 0);
-}
-
-void trampQueryR(void)
-{
-    trampQuery('r');
-}
-
-void trampQueryV(void)
-{
-    trampQuery('v');
-}
-
-void trampQueryS(void)
-{
-    trampQuery('s');
-}
-
-static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
-{
-    UNUSED(vtxDevice);
-
-    static timeUs_t lastQueryTimeUs = 0;
-    static bool initSettingsDoneFlag = false;
-
-#ifdef TRAMP_DEBUG
-    static uint16_t debugFreqReqCounter = 0;
-    static uint16_t debugPowReqCounter = 0;
-#endif
-
-    if (trampStatus == TRAMP_STATUS_BAD_DEVICE) {
-        return;
-    }
-
-    const char replyCode = trampReceive(currentTimeUs);
-
-#ifdef TRAMP_DEBUG
-    debug[0] = trampStatus;
-#endif
-
-    switch (replyCode) {
-    case 'r':
-        if (trampStatus <= TRAMP_STATUS_OFFLINE) {
-            trampStatus = TRAMP_STATUS_ONLINE;
-
-            // once device is ready enter vtx settings
-            if (!initSettingsDoneFlag) {
-                initSettingsDoneFlag = true;
-                // if vtx_band!=0 then enter 'vtx_band/chan' values (and power)
-            }
-        }
-        break;
-
-    case 'v':
-         if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) {
-             trampStatus = TRAMP_STATUS_SET_FREQ_PW;
-         }
-         break;
-    }
-
-    switch (trampStatus) {
-
-    case TRAMP_STATUS_OFFLINE:
-    case TRAMP_STATUS_ONLINE:
-        if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
-
-            if (trampStatus == TRAMP_STATUS_OFFLINE) {
-                trampQueryR();
-            } else {
-                static unsigned int cnt = 0;
-                if (((cnt++) & 1) == 0) {
-                    trampQueryV();
-                } else {
-                    trampQueryS();
-                }
-            }
-
-            lastQueryTimeUs = currentTimeUs;
-        }
-        break;
-
-    case TRAMP_STATUS_SET_FREQ_PW:
-        {
-            bool done = true;
-            if (trampConfFreq && trampFreqRetries && (trampConfFreq != trampData.curFreq)) {
-                trampSendFreq(trampConfFreq);
-                trampFreqRetries--;
-#ifdef TRAMP_DEBUG
-                debugFreqReqCounter++;
-#endif
-                done = false;
-            } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampData.configuredPower)) {
-                trampSendRFPower(trampConfPower);
-                trampPowerRetries--;
-#ifdef TRAMP_DEBUG
-                debugPowReqCounter++;
-#endif
-                done = false;
-            }
-
-            if (!done) {
-                trampStatus = TRAMP_STATUS_CHECK_FREQ_PW;
-
-                // delay next status query by 300ms
-                lastQueryTimeUs = currentTimeUs + 300 * 1000;
-            } else {
-                // everything has been done, let's return to original state
-                trampStatus = TRAMP_STATUS_ONLINE;
-                // reset configuration value in case it failed (no more retries)
-                trampConfFreq  = trampData.curFreq;
-                trampConfPower = trampData.power;
-                trampFreqRetries = trampPowerRetries = 0;
-            }
-        }
-        break;
-
-    case TRAMP_STATUS_CHECK_FREQ_PW:
-        if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) {
-            trampQueryV();
-            lastQueryTimeUs = currentTimeUs;
-        }
-        break;
-
-    default:
-        break;
-    }
-
-#ifdef TRAMP_DEBUG
-    debug[1] = debugFreqReqCounter;
-    debug[2] = debugPowReqCounter;
-    debug[3] = 0;
-#endif
-}
-
-
-// Interface to common VTX API
-
-static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice)
+static vtxDevType_e impl_GetDeviceType(const vtxDevice_t *vtxDevice)
 {
     UNUSED(vtxDevice);
     return VTXDEV_TRAMP;
 }
 
-static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice)
+static bool impl_IsReady(const vtxDevice_t *vtxDevice)
 {
-    return vtxDevice!=NULL && trampStatus > TRAMP_STATUS_OFFLINE;
+    return vtxDevice != NULL && vtxState.port != NULL && vtxState.protoState >= VTX_STATE_IDLE;
 }
 
-static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
+static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
 {
     UNUSED(vtxDevice);
-    if (trampValidateBandAndChannel(band, channel)) {
-        trampSetBandAndChannel(band, channel);
-        trampCommitChanges();
+
+    if (!impl_IsReady(vtxDevice)) {
+        return;
+    }
+
+    // TRAMP is 5.8 GHz only
+    uint16_t newFreqMhz  = vtx58_Bandchan2Freq(band, channel);
+
+    if (newFreqMhz < vtxState.capabilities.freqMin || newFreqMhz > vtxState.capabilities.freqMax) {
+        return false;
+    }
+
+    // Cache band and channel
+    vtxState.request.band = band;
+    vtxState.request.channel = channel;
+    vtxState.request.freq = newFreqMhz;
+    vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
+}
+
+static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index)
+{
+    UNUSED(vtxDevice);
+
+    if (!impl_IsReady(vtxDevice) || index < 1 || index > vtxState.metadata.powerTableCount) {
+        return;
+    }
+
+    unsigned reqPower = vtxState.metadata.powerTablePtr[index - 1];
+
+    // Cap the power to the max capability of the VTX
+    vtxState.request.power = MIN(reqPower, vtxState.capabilities.powerMax);
+    vtxState.request.powerIndex = index;
+
+    vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
+}
+
+static void impl_SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
+{
+    UNUSED(vtxDevice);
+
+    if (onoff == 0) {
+        vtxState.updateReqMask |= VTX_UPDATE_REQ_PITMODE;
     }
 }
 
-static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
+static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 {
-    UNUSED(vtxDevice);
-    trampDevSetPowerByIndex(index);
-}
-
-static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
-{
-    UNUSED(vtxDevice);
-    trampSetPitMode(onoff);
-}
-
-static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
-{
-    if (!vtxTrampIsReady(vtxDevice)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
     // if in user-freq mode then report band as zero
-    *pBand = trampData.setByFreqFlag ? 0 : trampData.band;
-    *pChannel = trampData.channel;
+    *pBand = vtxState.request.band;
+    *pChannel = vtxState.request.channel;
     return true;
 }
 
-static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
+static bool impl_GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 {
-    if (!vtxTrampIsReady(vtxDevice)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    if (trampData.configuredPower > 0) {
-        for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
-            if (trampData.configuredPower <= trampPowerTable[i]) {
-                *pIndex = i + 1;
-                break;
-            }
-        }
-    }
+    *pIndex = vtxState.request.powerIndex;
 
     return true;
 }
 
-static bool vtxTrampGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
+static bool impl_GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 {
-    if (!vtxTrampIsReady(vtxDevice)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    *pOnOff = trampData.pitMode;
+    *pOnOff = vtxState.state.pitMode ? 1 : 0;
     return true;
 }
 
-static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
+static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
 {
-    if (!vtxTrampIsReady(vtxDevice)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    *pFreq = trampData.curFreq;
+    *pFreq = vtxState.request.freq;
     return true;
 }
 
-static bool vtxTrampGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
+static bool impl_GetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
 {
-    uint8_t powerIndex;
-    if (!vtxTrampGetPowerIndex(vtxDevice, &powerIndex)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    *pIndex = trampData.configuredPower ? powerIndex : 0;
-    *pPowerMw = trampData.configuredPower;
+    *pIndex = vtxState.request.powerIndex;
+    *pPowerMw = vtxState.request.power;
     return true;
 }
 
-static bool vtxTrampGetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
+static bool impl_GetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
 {
-    uint8_t powerIndex; 
-    uint16_t powerMw;
-
-    if (!vtxTrampGetPower(vtxDevice, &powerIndex, &powerMw)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    pOsdInfo->band = trampData.setByFreqFlag ? 0 : trampData.band;
-    pOsdInfo->channel = trampData.channel;
-    pOsdInfo->frequency = trampData.curFreq;
-    pOsdInfo->powerIndex = powerIndex;
-    pOsdInfo->powerMilliwatt = powerMw;
-    pOsdInfo->bandLetter = vtx58BandNames[pOsdInfo->band][0];
-    pOsdInfo->bandName = vtx58BandNames[pOsdInfo->band];
-    pOsdInfo->channelName = vtx58ChannelNames[pOsdInfo->channel];
-    pOsdInfo->powerIndexLetter = '0' + powerIndex;
+    pOsdInfo->band = vtxState.request.band;
+    pOsdInfo->channel = vtxState.request.channel;
+    pOsdInfo->frequency = vtxState.request.freq;
+    pOsdInfo->powerIndex = vtxState.request.powerIndex;
+    pOsdInfo->powerMilliwatt = vtxState.request.power;
+    pOsdInfo->bandLetter = vtx58BandNames[vtxState.request.band][0];
+    pOsdInfo->bandName = vtx58BandNames[vtxState.request.band];
+    pOsdInfo->channelName = vtx58ChannelNames[vtxState.request.channel];
+    pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex;
     return true;
 }
 
-
-static const vtxVTable_t trampVTable = {
-    .process = vtxTrampProcess,
-    .getDeviceType = vtxTrampGetDeviceType,
-    .isReady = vtxTrampIsReady,
-    .setBandAndChannel = vtxTrampSetBandAndChannel,
-    .setPowerByIndex = vtxTrampSetPowerByIndex,
-    .setPitMode = vtxTrampSetPitMode,
-    .getBandAndChannel = vtxTrampGetBandAndChannel,
-    .getPowerIndex = vtxTrampGetPowerIndex,
-    .getPitMode = vtxTrampGetPitMode,
-    .getFrequency = vtxTrampGetFreq,
-    .getPower = vtxTrampGetPower,
-    .getOsdInfo = vtxTrampGetOsdInfo,
+/*****************************************************************************/
+static const vtxVTable_t impl_vtxVTable = {
+    .process = impl_Process,
+    .getDeviceType = impl_GetDeviceType,
+    .isReady = impl_IsReady,
+    .setBandAndChannel = impl_SetBandAndChannel,
+    .setPowerByIndex = impl_SetPowerByIndex,
+    .setPitMode = impl_SetPitMode,
+    .getBandAndChannel = impl_GetBandAndChannel,
+    .getPowerIndex = impl_GetPowerIndex,
+    .getPitMode = impl_GetPitMode,
+    .getFrequency = impl_GetFreq,
+    .getPower = impl_GetPower,
+    .getOsdInfo = impl_GetOsdInfo,
 };
 
+static vtxDevice_t impl_vtxDevice = {
+    .vTable = &impl_vtxVTable,
+    .capability.bandCount = VTX_TRAMP_5G8_BAND_COUNT,
+    .capability.channelCount = VTX_TRAMP_5G8_CHANNEL_COUNT,
+    .capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT,
+    .capability.bandNames = (char **)vtx58BandNames,
+    .capability.channelNames = (char **)vtx58ChannelNames,
+    .capability.powerNames = NULL,
+};
+
+const uint16_t trampPowerTable_200[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 200, 200 };
+const char * const trampPowerNames_200[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "200", "200" };
+
+const uint16_t trampPowerTable_400[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 400 };
+const char * const trampPowerNames_400[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "400" };
+
+const uint16_t trampPowerTable_600[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 600 };
+const char * const trampPowerNames_600[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "600" };
+
+const uint16_t trampPowerTable_800[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 500, 800 };
+const char * const trampPowerNames_800[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "500", "800" };
+
+static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
+{
+    if (maxPower >= 800) {
+        // Max power 800mW: Use 25, 100, 200, 500, 800 table
+        vtxState.metadata.powerTablePtr  = trampPowerTable_800;
+        vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT;
+        
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_800;
+        impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT;
+    }
+    else if (maxPower >= 600) {
+        // Max power 600mW: Use 25, 100, 200, 400, 600 table
+        vtxState.metadata.powerTablePtr  = trampPowerTable_600;
+        vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT;
+
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_600;
+        impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT;
+    }
+    else if (maxPower >= 400) {
+        // Max power 400mW: Use 25, 100, 200, 400 table
+        vtxState.metadata.powerTablePtr  = trampPowerTable_400;
+        vtxState.metadata.powerTableCount = 4;
+
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_400;
+        impl_vtxDevice.capability.powerCount = 4;
+    }
+    else if (maxPower >= 200) {
+        // Max power 200mW: Use 25, 100, 200 table
+        vtxState.metadata.powerTablePtr  = trampPowerTable_200;
+        vtxState.metadata.powerTableCount = 3;
+
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_200;
+        impl_vtxDevice.capability.powerCount = 3;
+    }
+    else {
+        // Default to standard TRAMP 600mW VTX
+        vtxState.metadata.powerTablePtr  = trampPowerTable_600;
+        vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT;
+
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_600;
+        impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT;
+    }
+
+}
 
 bool vtxTrampInit(void)
 {
@@ -609,17 +601,19 @@ bool vtxTrampInit(void)
     if (portConfig) {
         portOptions_t portOptions = 0;
         portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
-
-        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
+        vtxState.port = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
     }
 
-    if (!trampSerialPort) {
+    if (!vtxState.port) {
         return false;
     }
 
-    vtxCommonSetDevice(&vtxTramp);
+    vtxProtoUpdatePowerMetadata(600);
+    vtxCommonSetDevice(&impl_vtxDevice);
+
+    vtxState.protoState = VTX_STATE_RESET;
 
     return true;
 }
 
-#endif // VTX_TRAMP
+#endif
diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h
index a9c7cd04b2..3d2643c194 100644
--- a/src/main/io/vtx_tramp.h
+++ b/src/main/io/vtx_tramp.h
@@ -19,38 +19,13 @@
 
 #include <stdint.h>
 
-#define VTX_TRAMP_MIN_BAND 1
-#define VTX_TRAMP_MAX_BAND 5
-#define VTX_TRAMP_MIN_CHANNEL 1
-#define VTX_TRAMP_MAX_CHANNEL 8
+#define VTX_TRAMP_5G8_BAND_COUNT        5
+#define VTX_TRAMP_5G8_CHANNEL_COUNT     8
 
-#define VTX_TRAMP_BAND_COUNT (VTX_TRAMP_MAX_BAND - VTX_TRAMP_MIN_BAND + 1)
-#define VTX_TRAMP_CHANNEL_COUNT (VTX_TRAMP_MAX_CHANNEL - VTX_TRAMP_MIN_CHANNEL + 1)
-
-#define VTX_TRAMP_POWER_COUNT 5
+#define VTX_TRAMP_MAX_POWER_COUNT 5
 #define VTX_TRAMP_DEFAULT_POWER 1
 
 #define VTX_TRAMP_MIN_FREQUENCY_MHZ 5000             //min freq in MHz
 #define VTX_TRAMP_MAX_FREQUENCY_MHZ 5999             //max freq in MHz
 
-extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT];
-extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1];
-
-typedef struct trampData_s {
-    bool setByFreqFlag;  //false = set via band/channel
-    uint8_t band;
-    uint8_t channel;
-    uint16_t power;       // Actual transmitting power
-    uint16_t curFreq;
-    uint16_t configuredPower; // Configured transmitting power
-    int16_t temperature;
-    uint8_t pitMode;
-} trampData_t;
-
-extern trampData_t trampData;
-
 bool vtxTrampInit(void);
-bool trampCommitChanges(void);
-void trampSetPitMode(uint8_t onoff);
-void trampSetBandAndChannel(uint8_t band, uint8_t channel);
-void trampSetRFPower(uint16_t level);
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index fd6a7c5e27..0367bc5a3b 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -60,6 +60,7 @@
 #define MSP2_INAV_GLOBAL_FUNCTIONS              0x2024
 #define MSP2_INAV_SET_GLOBAL_FUNCTIONS          0x2025
 #define MSP2_INAV_LOGIC_CONDITIONS_STATUS       0x2026
+#define MSP2_INAV_GVAR_STATUS                   0x2027
 
 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index b57c3a4c75..1a50162737 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -268,8 +268,14 @@ static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int l
 #define JUMBO_FRAME_SIZE_LIMIT 255
 static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, const uint8_t * data, int dataLen, const uint8_t * crc, int crcLen)
 {
+    // MSP port might be turned into a CLI port, which will make
+    // msp->port become NULL.
+    serialPort_t *port = msp->port;
+    if (!port) {
+        return 0;
+    }
     // VSP MSP port might be unconnected. To prevent blocking - check if it's connected first
-    if (!serialIsConnected(msp->port)) {
+    if (!serialIsConnected(port)) {
         return 0;
     }
 
@@ -278,15 +284,15 @@ static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, c
     //     this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care)
     //  b) Response fits into TX buffer
     const int totalFrameLength = hdrLen + dataLen + crcLen;
-    if (!isSerialTransmitBufferEmpty(msp->port) && ((int)serialTxBytesFree(msp->port) < totalFrameLength))
+    if (!isSerialTransmitBufferEmpty(port) && ((int)serialTxBytesFree(port) < totalFrameLength))
         return 0;
 
     // Transmit frame
-    serialBeginWrite(msp->port);
-    serialWriteBuf(msp->port, hdr, hdrLen);
-    serialWriteBuf(msp->port, data, dataLen);
-    serialWriteBuf(msp->port, crc, crcLen);
-    serialEndWrite(msp->port);
+    serialBeginWrite(port);
+    serialWriteBuf(port, hdr, hdrLen);
+    serialWriteBuf(port, data, dataLen);
+    serialWriteBuf(port, crc, crcLen);
+    serialEndWrite(port);
 
     return totalFrameLength;
 }
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 634aae2c04..86855887c0 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -80,7 +80,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS];
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -143,6 +143,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .max_dive_angle = 15,                   // degrees
         .cruise_throttle = 1400,
         .cruise_speed = 0,                      // cm/s
+        .control_smoothness = 0,
         .max_throttle = 1700,
         .min_throttle = 1200,
         .pitch_to_throttle = 10,                // pwm units per degree of pitch (10pwm units ~ 1% throttle)
@@ -198,6 +199,10 @@ static void setupAltitudeController(void);
 static void resetHeadingController(void);
 void resetGCSFlags(void);
 
+static void setupJumpCounters(void);
+static void resetJumpCounter(void);
+static void clearJumpCounters(void);
+
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
 void calculateInitialHoldPosition(fpVector3_t * pos);
@@ -232,6 +237,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState);
@@ -666,6 +672,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .onEvent = {
             [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_WAYPOINT_REACHED,   // re-process state
             [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_NEXT,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
@@ -678,6 +685,27 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         }
     },
 
+    [NAV_STATE_WAYPOINT_HOLD_TIME] = {
+        .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME,                             // There is no state for timed hold?
+        .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
+        .timeoutMs = 10,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
+        .mwState = MW_NAV_STATE_HOLD_TIMED,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_WAYPOINT_HOLD_TIME,     // re-process the state
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_NEXT,          // successfully reached waypoint
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
+        }
+    },
+
     [NAV_STATE_WAYPOINT_RTH_LAND] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
@@ -717,7 +745,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_WP_ENROUTE,
         .mwError = MW_NAV_ERROR_FINISH,
@@ -1117,6 +1145,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
+    if (!STATE(ALTITUDE_CONTROL)) {
+        //If altitude control is not a thing, switch to RTH in progress instead
+        return NAV_FSM_EVENT_SUCCESS; //Will cause NAV_STATE_RTH_HEAD_HOME
+    }
+
     // If we have valid pos sensor OR we are configured to ignore GPS loss
     if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
         const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
@@ -1222,6 +1255,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
+    //On ROVER and BOAT we immediately switch to the next event
+    if (!STATE(ALTITUDE_CONTROL)) {
+        return NAV_FSM_EVENT_SUCCESS;
+    }
+
     // If position ok OR within valid timeout - continue
     if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
 
@@ -1284,6 +1322,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
 {
     UNUSED(previousState);
 
+    //On ROVER and BOAT we immediately switch to the next event
+    if (!STATE(ALTITUDE_CONTROL)) {
+        return NAV_FSM_EVENT_SUCCESS;
+    }
+
     if (!ARMING_FLAG(ARMED)) {
         return NAV_FSM_EVENT_SUCCESS;
     }
@@ -1327,7 +1370,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigatio
 {
     UNUSED(previousState);
 
-    if (navConfig()->general.flags.disarm_on_landing) {
+    //On ROVER and BOAT disarm immediately
+    if (!STATE(ALTITUDE_CONTROL) || navConfig()->general.flags.disarm_on_landing) {
         disarm(DISARM_NAVIGATION);
     }
 
@@ -1338,11 +1382,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
 {
     // Stay in this state
     UNUSED(previousState);
-    updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL);  // FIXME
+
+    if (STATE(ALTITUDE_CONTROL)) {
+        updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL);  // FIXME
+    }
 
     // Prevent I-terms growing when already landed
     pidResetErrorAccumulators();
-
     return NAV_FSM_EVENT_NONE;
 }
 
@@ -1360,7 +1406,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
         // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
         resetAltitudeController(false);
         setupAltitudeController();
-
+/*
+  Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
+  Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
+*/
+        setupJumpCounters();
         posControl.activeWaypointIndex = 0;
         return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
     }
@@ -1372,15 +1422,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
     UNUSED(previousState);
 
     switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
+        case NAV_WP_ACTION_HOLD_TIME:
         case NAV_WP_ACTION_WAYPOINT:
+        case NAV_WP_ACTION_LAND:
             calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
             posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
             posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
             return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
 
+                // We use p3 as the volatile jump counter (p2 is the static value)
         case NAV_WP_ACTION_JUMP:
-            if(posControl.waypointList[posControl.activeWaypointIndex].p2 != -1){
-                if(posControl.waypointList[posControl.activeWaypointIndex].p2 == 0){
+            if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
+                if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
+                    resetJumpCounter();
                     const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
                           (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
 
@@ -1395,11 +1449,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
                 }
                 else
                 {
-                    posControl.waypointList[posControl.activeWaypointIndex].p2--;
+                    posControl.waypointList[posControl.activeWaypointIndex].p3--;
                 }
             }
 
-            posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 - 1;
+            posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;
 
             return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
 
@@ -1420,7 +1474,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
     // If no position sensor available - land immediately
     if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
         switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
+            case NAV_WP_ACTION_HOLD_TIME:
             case NAV_WP_ACTION_WAYPOINT:
+            case NAV_WP_ACTION_LAND:
                 if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
                     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
                 }
@@ -1444,7 +1500,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
                 }
                 else {
-                    setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
+                    if(navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY))
+                        setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
+                    else
+                        setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
                     setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z);
                     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
                 }
@@ -1480,11 +1539,34 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
             else {
                 return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_WAYPOINT_NEXT
             }
+
+        case NAV_WP_ACTION_LAND:
+            return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
+
+        case NAV_WP_ACTION_HOLD_TIME:
+            // Save the current time for the time the waypoint was reached
+            posControl.wpReachedTime = millis();
+            return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
     }
 
     UNREACHABLE();
 }
 
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+
+    timeMs_t currentTime = millis();
+
+    if(posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0)
+        return NAV_FSM_EVENT_SUCCESS;
+
+    if(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)
+        return NAV_FSM_EVENT_SUCCESS;
+
+    return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
+}
+
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
@@ -1522,6 +1604,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
 {
     UNUSED(previousState);
 
+    clearJumpCounters();
     // If no position sensor available - land immediately
     if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
         return NAV_FSM_EVENT_NONE;
@@ -1587,7 +1670,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
 
     //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
     if (feature(FEATURE_FW_LAUNCH)) {
-        throttleStatus_e throttleStatus = calculateThrottleStatus();
+        throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
         if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
             abortFixedWingLaunch();
             return NAV_FSM_EVENT_SWITCH_TO_IDLE;
@@ -1825,11 +1908,6 @@ float navPidApply3(
         }
     }
 
-    /*
-     * Limit both output and Iterm to limit windup
-     */
-    pid->integrator = constrain(pid->integrator, outMin, outMax);
-
     return outValConstrained;
 }
 
@@ -2512,6 +2590,36 @@ static bool adjustAltitudeFromRCInput(void)
     }
 }
 
+/*-----------------------------------------------------------
+ * Jump Counter support functions
+ *-----------------------------------------------------------*/
+static void setupJumpCounters(void)
+{
+    for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
+        if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
+            posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
+        }
+    }
+}
+
+static void resetJumpCounter(void)
+{
+        // reset the volatile counter from the set / static value
+    posControl.waypointList[posControl.activeWaypointIndex].p3 =
+        posControl.waypointList[posControl.activeWaypointIndex].p2;
+}
+
+static void clearJumpCounters(void)
+{
+    for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
+        if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
+            posControl.waypointList[wp].p3 = 0;
+        }
+    }
+}
+
+
+
 /*-----------------------------------------------------------
  * Heading controller (pass-through to MAG mode)
  *-----------------------------------------------------------*/
@@ -2606,10 +2714,14 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
         wpData->lon = wpLLH.lon;
         wpData->alt = wpLLH.alt;
     }
-    // WP #1 - #15 - common waypoints - pre-programmed mission
+    // WP #1 - #60 - common waypoints - pre-programmed mission
     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
         if (wpNumber <= posControl.waypointCount) {
             *wpData = posControl.waypointList[wpNumber - 1];
+            if(wpData->action == NAV_WP_ACTION_JUMP) {
+                wpData->p1 += 1; // make WP # (vice index)
+            }
+
         }
     }
 }
@@ -2653,10 +2765,13 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
     }
     // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
-        if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH) {
+        if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND) {
             // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
             if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
                 posControl.waypointList[wpNumber - 1] = *wpData;
+                if(wpData->action == NAV_WP_ACTION_JUMP) {
+                    posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
+                }
                 posControl.waypointCount = wpNumber;
                 posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
             }
@@ -2780,6 +2895,11 @@ bool isApproachingLastWaypoint(void)
     }
 }
 
+bool isWaypointWait(void)
+{
+    return NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
+}
+
 float getActiveWaypointSpeed(void)
 {
     if (posControl.flags.isAdjustingPosition) {
@@ -2790,8 +2910,13 @@ float getActiveWaypointSpeed(void)
         uint16_t waypointSpeed = navConfig()->general.max_auto_speed;
 
         if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
-            if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) {
-                const float wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
+            if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
+                float wpSpecificSpeed = 0.0f;
+                if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
+                    wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
+                else
+                    wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
+
                 if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
                     waypointSpeed = wpSpecificSpeed;
                 }
@@ -2873,7 +2998,9 @@ void applyWaypointNavigationAndAltitudeHold(void)
 
     /* Process controllers */
     navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
-    if (STATE(FIXED_WING_LEGACY)) {
+    if (STATE(ROVER) || STATE(BOAT)) {
+        applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
+    } else if (STATE(FIXED_WING_LEGACY)) {
         applyFixedWingNavigationController(navStateFlags, currentTimeUs);
     }
     else {
@@ -2960,7 +3087,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
             else {
                 // If we were in LAUNCH mode - force switch to IDLE only if the throttle is low
                 if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
-                    throttleStatus_e throttleStatus = calculateThrottleStatus();
+                    throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
                     if (throttleStatus != THROTTLE_LOW)
                         return NAV_FSM_EVENT_NONE;
                     else
@@ -3127,16 +3254,27 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
         }
     }
 
-    // Don't allow arming if any of JUMP waypoint has invalid settings
+        /*
+         * Don't allow arming if any of JUMP waypoint has invalid settings
+         * First WP can't be JUMP
+         * Can't jump to immediately adjacent WPs (pointless)
+         * Can't jump beyond WP list
+         * Only jump to geo-referenced WP types
+         */
     if (posControl.waypointCount > 0) {
-    for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
-       if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
-           if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
-               return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
+        for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
+            if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
+                if((wp == 0) || ((posControl.waypointList[wp].p1 > (wp-2)) && (posControl.waypointList[wp].p1 < (wp+2)) ) || (posControl.waypointList[wp].p1 >=  posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)) {
+                    return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
+                }
+                    /* check for target geo-ref sanity */
+                uint16_t target = posControl.waypointList[wp].p1;
+                if(!(posControl.waypointList[target].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[target].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[target].action == NAV_WP_ACTION_LAND)) {
+                    return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
+                }
             }
         }
     }
-    }
 
     return NAV_ARMING_BLOCKER_NONE;
 }
@@ -3280,7 +3418,7 @@ void navigationUsePIDs(void)
                                         0.0f,
                                         NAV_DTERM_CUT_HZ
     );
-    
+
     navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
@@ -3395,12 +3533,15 @@ bool navigationIsFlyingAutonomousMode(void)
 
 bool navigationRTHAllowsLanding(void)
 {
+    if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)
+        return true;
+
     navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
     return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
         (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));
 }
 
-bool FAST_CODE isNavLaunchEnabled(void)
+bool isNavLaunchEnabled(void)
 {
     return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH);
 }
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index d20f3b60e9..57927b0687 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -198,6 +198,7 @@ typedef struct navConfig_s {
         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
         uint16_t cruise_throttle;            // Cruise throttle
         uint16_t cruise_speed;               // Speed at cruise throttle (cm/s), used for time/distance left before RTH
+        uint8_t control_smoothness;          // The amount of smoothing to apply to controls for navigation
         uint16_t min_throttle;               // Minimum allowed throttle in auto mode
         uint16_t max_throttle;               // Maximum allowed throttle in auto mode
         uint8_t  pitch_to_throttle;          // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
@@ -233,9 +234,11 @@ typedef struct gpsOrigin_s {
 } gpsOrigin_t;
 
 typedef enum {
-    NAV_WP_ACTION_WAYPOINT = 0x01,
-    NAV_WP_ACTION_JUMP     = 0x06, 
-    NAV_WP_ACTION_RTH      = 0x04
+    NAV_WP_ACTION_WAYPOINT  = 0x01,
+    NAV_WP_ACTION_HOLD_TIME = 0x03,
+    NAV_WP_ACTION_RTH       = 0x04,
+    NAV_WP_ACTION_JUMP      = 0x06,
+    NAV_WP_ACTION_LAND      = 0x08
 } navWaypointActions_e;
 
 typedef enum {
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 8dbabf6390..389d934b57 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -51,6 +51,9 @@
 
 #include "rx/rx.h"
 
+// Base frequencies for smoothing pitch and roll 
+#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ     2.0f      
+#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ     10.0f
 
 // If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
 #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN        1.5f
@@ -67,6 +70,14 @@ static bool isAutoThrottleManuallyIncreased = false;
 static int32_t navHeadingError;
 static int8_t loiterDirYaw = 1;
 
+// Calculates the cutoff frequency for smoothing out roll/pitch commands 
+// control_smoothness valid range from 0 to 9
+// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz 
+static float getSmoothnessCutoffFreq(float baseFreq)
+{
+    uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
+    return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f;
+}
 
 /*-----------------------------------------------------------
  * Altitude controller
@@ -136,7 +147,9 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 
     // PID controller to translate energy balance error [J] into pitch angle [decideg]
     float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f);
-    targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
+
+    // Apply low-pass filter to prevent rapid correction
+    targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 
     // Reconstrain pitch angle ( >0 - climb, <0 - dive)
     targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
@@ -203,6 +216,9 @@ bool adjustFixedWingHeadingFromRCInput(void)
 static fpVector3_t virtualDesiredPosition;
 static pt1Filter_t fwPosControllerCorrectionFilterState;
 
+/*
+ * TODO Currently this function resets both FixedWing and Rover & Boat position controller 
+ */
 void resetFixedWingPositionController(void)
 {
     virtualDesiredPosition.x = 0;
@@ -243,7 +259,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
 
     // If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
     #define TAN_15DEG    0.26795f
-    bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
+    bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
                                             && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
                                             && (distanceToActualTarget > 50.0f)
                                             && !FLIGHT_MODE(NAV_CRUISE_MODE);
@@ -286,6 +302,34 @@ bool adjustFixedWingPositionFromRCInput(void)
     return (rcRollAdjustment);
 }
 
+float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing) {
+    static float limit = 0.0f;
+
+    if (limit == 0.0f) {
+        limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
+    }
+
+    const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
+
+    const float yawAdjustment = navPidApply2(
+        &posControl.pids.fw_heading, 
+        0, 
+        applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100), 
+        US2S(deltaMicros),
+        -limit,
+        limit,
+        yawPidFlags
+        ) / 100.0f;
+
+    DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
+    DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
+    DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
+    DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
+    DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
+
+    return yawAdjustment;
+}
+
 static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
 {
     static timeUs_t previousTimeMonitoringUpdate;
@@ -336,7 +380,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
                                         pidFlags);
 
     // Apply low-pass filter to prevent rapid correction
-    rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
+    rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, getSmoothnessCutoffFreq(NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 
     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
@@ -346,32 +390,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
      * It is working in relative mode and we aim to keep error at zero
      */
     if (STATE(FW_HEADING_USE_YAW)) {
-
-        static float limit = 0.0f;
-
-        if (limit == 0.0f) {
-            limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
-        }
-
-        const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
-
-        float yawAdjustment = navPidApply2(
-            &posControl.pids.fw_heading, 
-            0, 
-            applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100), 
-            US2S(deltaMicros),
-            -limit,
-            limit,
-            yawPidFlags
-            ) / 100.0f;
-
-        DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
-        DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
-        DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
-        DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
-        DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
-
-        posControl.rcAdjustment[YAW] = yawAdjustment;
+        posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
     } else {
         posControl.rcAdjustment[YAW] = 0;
     }
diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c
index ebdd30b5ce..843d27f650 100755
--- a/src/main/navigation/navigation_fw_launch.c
+++ b/src/main/navigation/navigation_fw_launch.c
@@ -106,7 +106,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs)
     launchState.motorControlAllowed = false;
 }
 
-bool FAST_CODE isFixedWingLaunchDetected(void)
+bool isFixedWingLaunchDetected(void)
 {
     return launchState.launchDetected;
 }
@@ -117,7 +117,7 @@ void enableFixedWingLaunchController(timeUs_t currentTimeUs)
     launchState.motorControlAllowed = true;
 }
 
-bool FAST_CODE isFixedWingLaunchFinishedOrAborted(void)
+bool isFixedWingLaunchFinishedOrAborted(void)
 {
     return launchState.launchFinished;
 }
@@ -146,7 +146,7 @@ static void applyFixedWingLaunchIdleLogic(void)
     else
     {
         static float timeThrottleRaisedMs;
-        if (calculateThrottleStatus() == THROTTLE_LOW)
+        if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW)
         {
             timeThrottleRaisedMs = millis();
         }
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index b0130f3a11..b7bfa2e2d7 100755
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -31,6 +31,7 @@
 #include "common/axis.h"
 #include "common/maths.h"
 #include "common/filter.h"
+#include "common/utils.h"
 
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
@@ -163,7 +164,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
 
 void setupMulticopterAltitudeController(void)
 {
-    const throttleStatus_e throttleStatus = calculateThrottleStatus();
+    const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
 
     if (navConfig()->general.flags.use_thr_mid_for_althold) {
         altHoldThrottleRCZero = rcLookupThrottleMid();
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 3e2b9047a4..d4dbb9a2c6 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -823,7 +823,7 @@ void initializePositionEstimator(void)
  * Update estimator
  *  Update rate: loop rate (>100Hz)
  */
-void FAST_CODE NOINLINE updatePositionEstimator(void)
+void updatePositionEstimator(void)
 {
     static bool isInitialized = false;
 
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index e97d76311d..fcae1b7ded 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -30,8 +30,6 @@
 #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
 #define NAV_FW_CONTROL_MONITORING_RATE      2
-#define NAV_FW_PITCH_CUTOFF_FREQENCY_HZ     2       // low-pass filter on Z (pitch angle) for fixed wing
-#define NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ     10      // low-pass filter on roll correction for fixed wing
 #define NAV_DTERM_CUT_HZ                    10.0f
 #define NAV_ACCELERATION_XY_MAX             980.0f  // cm/s/s       // approx 45 deg lean angle
 
@@ -139,9 +137,11 @@ typedef enum {
 
     NAV_FSM_EVENT_STATE_SPECIFIC_1,             // State-specific event
     NAV_FSM_EVENT_STATE_SPECIFIC_2,             // State-specific event
+    NAV_FSM_EVENT_STATE_SPECIFIC_3,             // State-specific event
     NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1,
     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
+    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
 
     NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D,
     NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D,
@@ -198,6 +198,8 @@ typedef enum {
     NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE                      = 32,
     NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS                     = 33,
     NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING                       = 34,
+
+    NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME                        = 35,
 } navigationPersistentId_e;
 
 typedef enum {
@@ -224,6 +226,7 @@ typedef enum {
     NAV_STATE_WAYPOINT_PRE_ACTION,
     NAV_STATE_WAYPOINT_IN_PROGRESS,
     NAV_STATE_WAYPOINT_REACHED,
+    NAV_STATE_WAYPOINT_HOLD_TIME,
     NAV_STATE_WAYPOINT_NEXT,
     NAV_STATE_WAYPOINT_FINISHED,
     NAV_STATE_WAYPOINT_RTH_LAND,
@@ -271,6 +274,7 @@ typedef enum {
 
     /* Additional flags */
     NAV_CTL_LAND            = (1 << 14),
+    NAV_AUTO_WP_DONE        = (1 << 15),    //Waypoint mission reached the last waypoint and is idling
 } navigationFSMStateFlags_t;
 
 typedef struct {
@@ -357,6 +361,7 @@ typedef struct {
     float                       wpInitialAltitude; // Altitude at start of WP
     float                       wpInitialDistance; // Distance when starting flight to WP
     float                       wpDistance;        // Distance to active WP
+    timeMs_t                    wpReachedTime;     // Time the waypoint was reached
 
     /* Internals & statistics */
     int16_t                     rcAdjustment[4];
@@ -411,6 +416,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
 
 bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
 bool isWaypointMissed(const navWaypointPosition_t * waypoint);
+bool isWaypointWait(void);
 bool isApproachingLastWaypoint(void);
 float getActiveWaypointSpeed(void);
 
@@ -455,6 +461,8 @@ bool adjustFixedWingAltitudeFromRCInput(void);
 bool adjustFixedWingHeadingFromRCInput(void);
 bool adjustFixedWingPositionFromRCInput(void);
 
+void applyFixedWingPositionController(timeUs_t currentTimeUs);
+float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing);
 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
 
 void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
@@ -467,4 +475,9 @@ bool isFixedWingLaunchFinishedOrAborted(void);
 void abortFixedWingLaunch(void);
 void applyFixedWingLaunchController(timeUs_t currentTimeUs);
 
+/*
+ * Rover specific functions
+ */
+void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
+
 #endif
diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c
new file mode 100644
index 0000000000..1f0a01669a
--- /dev/null
+++ b/src/main/navigation/navigation_rover_boat.c
@@ -0,0 +1,146 @@
+/*
+ * 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_NAV
+
+#include "build/debug.h"
+#include "common/utils.h"
+#include "fc/rc_controls.h"
+#include "fc/config.h"
+#include "flight/mixer.h"
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+
+static bool isYawAdjustmentValid = false;
+static int32_t navHeadingError;
+
+static void update2DPositionHeadingController(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
+{
+    static timeUs_t previousTimeMonitoringUpdate;
+    static int32_t previousHeadingError;
+    static bool errorIsDecreasing;
+
+    int32_t targetBearing = calculateBearingToDestination(&posControl.desiredState.pos);
+
+    /*
+     * Calculate NAV heading error
+     * Units are centidegrees
+     */
+    navHeadingError = wrap_18000(targetBearing - posControl.actualState.yaw);
+
+    // Slow error monitoring (2Hz rate)
+    if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
+        // Check if error is decreasing over time
+        errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));
+
+        // Save values for next iteration
+        previousHeadingError = navHeadingError;
+        previousTimeMonitoringUpdate = currentTimeUs;
+    }
+
+    posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
+}
+
+void applyRoverBoatPositionController(timeUs_t currentTimeUs)
+{
+    static timeUs_t previousTimePositionUpdate;         // Occurs @ GPS update rate
+    static timeUs_t previousTimeUpdate;                 // Occurs @ looptime rate
+
+    const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
+    previousTimeUpdate = currentTimeUs;
+
+    // If last position update was too long in the past - ignore it (likely restarting altitude controller)
+    if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+        previousTimeUpdate = currentTimeUs;
+        previousTimePositionUpdate = currentTimeUs;
+        resetFixedWingPositionController();
+        return;
+    }
+
+    // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
+    if ((posControl.flags.estPosStatus >= EST_USABLE)) {
+        // If we have new position - update velocity and acceleration controllers
+        if (posControl.flags.horizontalPositionDataNew) {
+            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            previousTimePositionUpdate = currentTimeUs;
+
+            if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+                update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate);
+            } else {
+                resetFixedWingPositionController();
+            }
+
+            // Indicate that information is no longer usable
+            posControl.flags.horizontalPositionDataConsumed = 1;
+        }
+
+        isYawAdjustmentValid = true;
+    }
+    else {
+        isYawAdjustmentValid = false;
+    }
+}
+
+void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    rcCommand[ROLL] = 0;
+    rcCommand[PITCH] = 0;
+
+    if (navStateFlags & NAV_CTL_POS) {
+
+        if (navStateFlags & NAV_AUTO_WP_DONE) {
+            /*
+             * When WP mission is done, stop the motors
+             */
+            rcCommand[YAW] = 0;
+            rcCommand[THROTTLE] = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
+        } else {
+            if (isYawAdjustmentValid) {
+                rcCommand[YAW] = posControl.rcAdjustment[YAW];
+            }
+
+            rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
+        }
+    }
+}
+
+void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
+{
+    if (navStateFlags & NAV_CTL_EMERG) {
+        rcCommand[ROLL] = 0;
+        rcCommand[PITCH] = 0;
+        rcCommand[YAW] = 0;
+        rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
+    } else if (navStateFlags & NAV_CTL_POS) {
+        applyRoverBoatPositionController(currentTimeUs);
+        applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
+    }
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/platform.h b/src/main/platform.h
index 657b067480..110ea783d0 100644
--- a/src/main/platform.h
+++ b/src/main/platform.h
@@ -20,6 +20,7 @@
 #if defined(STM32F7)
 #include "stm32f7xx.h"
 #include "stm32f7xx_hal.h"
+#include "stm32f7xx_hal_rtc.h"
 #include "stm32f7xx_ll_spi.h"
 #include "stm32f7xx_ll_gpio.h"
 #include "stm32f7xx_ll_dma.h"
@@ -61,6 +62,7 @@
 
 #include "target/common.h"
 #include "target.h"
+#include "target/sanity_check.h"
 #include "target/common_post.h"
 
 // Remove the unaligned packed structure member pointer access warning
diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index 276bb956b9..6a0ad8d3f5 100755
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -21,7 +21,7 @@
 #include <string.h>
 
 #include "platform.h"
-
+FILE_COMPILE_FOR_SPEED
 #ifdef USE_SERIALRX_CRSF
 
 #include "build/build_config.h"
diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index e544e434e7..4b607373c4 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -21,6 +21,7 @@
 #include <string.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_SERIAL_RX)
 
@@ -38,6 +39,7 @@
 #include "telemetry/smartport.h"
 #endif
 
+#include "rx/frsky_crc.h"
 #include "rx/rx.h"
 #include "rx/sbus_channels.h"
 #include "rx/fport.h"
@@ -56,8 +58,6 @@
 #define FPORT_ESCAPE_CHAR 0x7D
 #define FPORT_ESCAPE_MASK 0x20
 
-#define FPORT_CRC_VALUE 0xFF
-
 #define FPORT_BAUDRATE 115200
 
 #define FPORT_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
@@ -238,18 +238,6 @@ static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
 }
 #endif
 
-static bool checkChecksum(uint8_t *data, uint8_t length)
-{
-    uint16_t checksum = 0;
-    for (unsigned i = 0; i < length; i++) {
-        checksum = checksum + *(uint8_t *)(data + i);
-    }
-
-    checksum = (checksum & 0xff) + (checksum >> 8);
-
-    return checksum == FPORT_CRC_VALUE;
-}
-
 static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
 #if defined(USE_TELEMETRY_SMARTPORT)
@@ -267,7 +255,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         if (frameLength != bufferLength - 2) {
             reportFrameError(DEBUG_FPORT_ERROR_SIZE);
         } else {
-            if (!checkChecksum(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
+            if (!frskyCheckSumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
                 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
             } else {
                 fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];
diff --git a/src/main/rx/frsky_crc.c b/src/main/rx/frsky_crc.c
new file mode 100644
index 0000000000..32411ecc39
--- /dev/null
+++ b/src/main/rx/frsky_crc.c
@@ -0,0 +1,53 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+FILE_COMPILE_FOR_SPEED
+
+#include "rx/frsky_crc.h"
+
+void frskyCheckSumStep(uint16_t *checksum, uint8_t byte)
+{
+    *checksum += byte;
+}
+
+void frskyCheckSumFini(uint16_t *checksum)
+{
+    while (*checksum > 0xFF) {
+        *checksum = (*checksum & 0xFF) + (*checksum >> 8);
+    }
+
+    *checksum = 0xFF - *checksum;
+}
+
+uint8_t frskyCheckSum(uint8_t *data, uint8_t length)
+{
+    uint16_t checksum = 0;
+    for (unsigned i = 0; i < length; i++) {
+        frskyCheckSumStep(&checksum, *data++);
+    }
+    frskyCheckSumFini(&checksum);
+    return checksum;
+}
+
+bool frskyCheckSumIsGood(uint8_t *data, uint8_t length)
+{
+    return !frskyCheckSum(data, length);
+}
diff --git a/src/main/rx/frsky_crc.h b/src/main/rx/frsky_crc.h
new file mode 100644
index 0000000000..c0a026ba47
--- /dev/null
+++ b/src/main/rx/frsky_crc.h
@@ -0,0 +1,24 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+uint8_t frskyCheckSum(uint8_t *data, uint8_t length);
+bool frskyCheckSumIsGood(uint8_t *data, uint8_t length);
+void frskyCheckSumStep(uint16_t *checksum, uint8_t byte);   // Add byte to checksum
+void frskyCheckSumFini(uint16_t *checksum);                 // Finalize checksum
diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c
index 04bc855697..a41e264c2a 100644
--- a/src/main/rx/sbus.c
+++ b/src/main/rx/sbus.c
@@ -21,6 +21,7 @@
 #include <string.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #ifdef USE_SERIAL_RX
 
@@ -52,21 +53,6 @@
  * time to send frame: 3ms.
  */
 
-#define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
-
-#define SBUS_FRAME_BEGIN_BYTE 0x0F
-
-#define SBUS_BAUDRATE       100000
-#define SBUS_BAUDRATE_FAST  200000
-
-#if !defined(SBUS_PORT_OPTIONS)
-#define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN)
-#endif
-
-#define SBUS_DIGITAL_CHANNEL_MIN 173
-#define SBUS_DIGITAL_CHANNEL_MAX 1812
-
-
 enum {
     DEBUG_SBUS_INTERFRAME_TIME = 0,
     DEBUG_SBUS_FRAME_FLAGS = 1,
@@ -79,19 +65,6 @@ typedef enum {
     STATE_SBUS_WAIT_SYNC
 } sbusDecoderState_e;
 
-typedef struct sbusFrame_s {
-    uint8_t syncByte;
-    sbusChannels_t channels;
-    /**
-     * The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
-     *
-     * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
-     * and
-     * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
-     */
-    uint8_t endByte;
-} __attribute__ ((__packed__)) sbusFrame_t;
-
 typedef struct sbusFrameData_s {
     sbusDecoderState_e state;
     volatile sbusFrame_t frame;
@@ -101,8 +74,6 @@ typedef struct sbusFrameData_s {
     timeUs_t lastActivityTimeUs;
 } sbusFrameData_t;
 
-STATIC_ASSERT(SBUS_FRAME_SIZE == sizeof(sbusFrame_t), SBUS_FRAME_SIZE_doesnt_match_sbusFrame_t);
-
 // Receive ISR callback
 static void sbusDataReceive(uint16_t c, void *data)
 {
diff --git a/src/main/rx/sbus_channels.c b/src/main/rx/sbus_channels.c
index 6e25ed719e..ba87343e84 100644
--- a/src/main/rx/sbus_channels.c
+++ b/src/main/rx/sbus_channels.c
@@ -24,18 +24,18 @@
 #ifdef USE_SERIAL_RX
 
 #include "common/utils.h"
+#include "common/maths.h"
 
-#include "rx/rx.h"
 #include "rx/sbus_channels.h"
 
-#define DEBUG_SBUS_FRAME_INTERVAL 3
-
 #define SBUS_FLAG_CHANNEL_17        (1 << 0)
 #define SBUS_FLAG_CHANNEL_18        (1 << 1)
 
 #define SBUS_DIGITAL_CHANNEL_MIN 173
 #define SBUS_DIGITAL_CHANNEL_MAX 1812
 
+STATIC_ASSERT(SBUS_FRAME_SIZE == sizeof(sbusFrame_t), SBUS_FRAME_SIZE_doesnt_match_sbusFrame_t);
+
 uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels)
 {
     uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
@@ -82,11 +82,21 @@ uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannel
     return RX_FRAME_COMPLETE;
 }
 
-static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
+uint16_t sbusDecodeChannelValue(uint16_t sbusValue)
 {
     // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
     // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
-    return (5 * rxRuntimeConfig->channelData[chan] / 8) + 880;
+    return (5 * constrain(sbusValue, SBUS_DIGITAL_CHANNEL_MIN, SBUS_DIGITAL_CHANNEL_MAX) / 8) + 880;
+}
+
+uint16_t sbusEncodeChannelValue(uint16_t rcValue)
+{
+    return constrain((((int)rcValue - 880) * 8 + 4) / 5, SBUS_DIGITAL_CHANNEL_MIN, SBUS_DIGITAL_CHANNEL_MAX);
+}
+
+static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
+{
+    return sbusDecodeChannelValue(rxRuntimeConfig->channelData[chan]);
 }
 
 void sbusChannelsInit(rxRuntimeConfig_t *rxRuntimeConfig)
diff --git a/src/main/rx/sbus_channels.h b/src/main/rx/sbus_channels.h
index 962e0fa9e2..1354d82fe3 100644
--- a/src/main/rx/sbus_channels.h
+++ b/src/main/rx/sbus_channels.h
@@ -18,12 +18,25 @@
 #pragma once
 
 #include <stdint.h>
+#include "rx/rx.h"
 
 #define SBUS_MAX_CHANNEL 18
 
 #define SBUS_FLAG_SIGNAL_LOSS       (1 << 2)
 #define SBUS_FLAG_FAILSAFE_ACTIVE   (1 << 3)
 
+#define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t)
+#define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
+
+#define SBUS_FRAME_BEGIN_BYTE 0x0F
+
+#define SBUS_BAUDRATE       100000
+#define SBUS_BAUDRATE_FAST  200000
+
+#if !defined(SBUS_PORT_OPTIONS)
+#define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN)
+#endif
+
 typedef struct sbusChannels_s {
     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
     unsigned int chan0 : 11;
@@ -45,7 +58,22 @@ typedef struct sbusChannels_s {
     uint8_t flags;
 } __attribute__((__packed__)) sbusChannels_t;
 
-#define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t)
+typedef struct sbusFrame_s {
+    uint8_t syncByte;
+    sbusChannels_t channels;
+    /**
+     * The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
+     *
+     * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
+     * and
+     * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
+     */
+    uint8_t endByte;
+} __attribute__ ((__packed__)) sbusFrame_t;
+
+
+uint16_t sbusDecodeChannelValue(uint16_t sbusValue);
+uint16_t sbusEncodeChannelValue(uint16_t rcValue);
 
 uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels);
 
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index ee83dd5fa0..f3de93c7b7 100755
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "scheduler.h"
 
 #include "build/build_config.h"
@@ -178,7 +180,7 @@ void setTaskEnabled(cfTaskId_e taskId, bool enabled)
     }
 }
 
-timeDelta_t FAST_CODE NOINLINE getTaskDeltaTime(cfTaskId_e taskId)
+timeDelta_t getTaskDeltaTime(cfTaskId_e taskId)
 {
     if (taskId == TASK_SELF) {
         return currentTask->taskLatestDeltaTime;
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index b9cf88bd21..a9d13ef5ef 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -118,6 +118,9 @@ typedef enum {
 #endif
 #ifdef USE_RPM_FILTER
     TASK_RPM_FILTER,
+#endif
+#ifdef USE_IRLOCK
+    TASK_IRLOCK,
 #endif
     /* Count of real tasks */
     TASK_COUNT,
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 5a1cdd8b6d..015d55f359 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -21,6 +21,9 @@
 #include <math.h>
 
 #include "platform.h"
+
+FILE_COMPILE_FOR_SPEED
+
 #include "build/debug.h"
 
 #include "common/axis.h"
@@ -112,9 +115,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 {
     accelerationSensor_e accHardware = ACC_NONE;
 
-#ifdef USE_ACC_ADXL345
-#endif
-
     dev->accAlign = ALIGN_DEFAULT;
 
     requestedSensors[SENSOR_INDEX_ACC] = accHardwareToUse;
@@ -122,12 +122,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
     switch (accHardwareToUse) {
     case ACC_AUTODETECT:
         FALLTHROUGH;
-#ifdef USE_ACC_ADXL345
+#ifdef USE_IMU_ADXL345
     case ACC_ADXL345: {
         if (adxl345Detect(dev)) {
-#ifdef ACC_ADXL345_ALIGN
-            dev->accAlign = ACC_ADXL345_ALIGN;
-#endif
             accHardware = ACC_ADXL345;
             break;
         }
@@ -139,12 +136,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
     FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_LSM303DLHC
+#ifdef USE_IMU_LSM303DLHC
     case ACC_LSM303DLHC:
         if (lsm303dlhcAccDetect(dev)) {
-#ifdef ACC_LSM303DLHC_ALIGN
-            dev->accAlign = ACC_LSM303DLHC_ALIGN;
-#endif
             accHardware = ACC_LSM303DLHC;
             break;
         }
@@ -155,12 +149,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_MPU6050
+#ifdef USE_IMU_MPU6050
     case ACC_MPU6050: // MPU6050
         if (mpu6050AccDetect(dev)) {
-#ifdef ACC_MPU6050_ALIGN
-            dev->accAlign = ACC_MPU6050_ALIGN;
-#endif
             accHardware = ACC_MPU6050;
             break;
         }
@@ -171,13 +162,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_MMA8452
+#ifdef USE_IMU_MMA8452
     case ACC_MMA8452: // MMA8452
-
         if (mma8452Detect(dev)) {
-#ifdef ACC_MMA8452_ALIGN
-            dev->accAlign = ACC_MMA8452_ALIGN;
-#endif
             accHardware = ACC_MMA8452;
             break;
         }
@@ -188,12 +175,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_BMA280
+#ifdef USE_IMU_BMA280
     case ACC_BMA280: // BMA280
         if (bma280Detect(dev)) {
-#ifdef ACC_BMA280_ALIGN
-            dev->accAlign = ACC_BMA280_ALIGN;
-#endif
             accHardware = ACC_BMA280;
             break;
         }
@@ -204,12 +188,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_MPU6000
+#ifdef USE_IMU_MPU6000
     case ACC_MPU6000:
         if (mpu6000AccDetect(dev)) {
-#ifdef ACC_MPU6000_ALIGN
-            dev->accAlign = ACC_MPU6000_ALIGN;
-#endif
             accHardware = ACC_MPU6000;
             break;
         }
@@ -220,12 +201,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#if defined(USE_ACC_MPU6500)
+#if defined(USE_IMU_MPU6500)
     case ACC_MPU6500:
         if (mpu6500AccDetect(dev)) {
-#ifdef ACC_MPU6500_ALIGN
-            dev->accAlign = ACC_MPU6500_ALIGN;
-#endif
             accHardware = ACC_MPU6500;
             break;
         }
@@ -236,12 +214,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#if defined(USE_ACC_MPU9250)
+#if defined(USE_IMU_MPU9250)
     case ACC_MPU9250:
         if (mpu9250AccDetect(dev)) {
-#ifdef ACC_MPU9250_ALIGN
-            dev->accAlign = ACC_MPU9250_ALIGN;
-#endif
             accHardware = ACC_MPU9250;
             break;
         }
@@ -252,12 +227,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#if defined(USE_ACC_BMI160)
+#if defined(USE_IMU_BMI160)
     case ACC_BMI160:
         if (bmi160AccDetect(dev)) {
-#ifdef ACC_BMI160_ALIGN
-            dev->accAlign = ACC_BMI160_ALIGN;
-#endif
             accHardware = ACC_BMI160;
             break;
         }
@@ -268,12 +240,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_ICM20689
+#ifdef USE_IMU_ICM20689
     case ACC_ICM20689:
         if (icm20689AccDetect(dev)) {
-#ifdef ACC_ICM20689_ALIGN
-            dev->accAlign = ACC_ICM20689_ALIGN;
-#endif
             accHardware = ACC_ICM20689;
             break;
         }
@@ -285,7 +254,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #endif
 
 
-#ifdef USE_FAKE_ACC
+#ifdef USE_IMU_FAKE
     case ACC_FAKE:
         if (fakeAccDetect(dev)) {
             accHardware = ACC_FAKE;
@@ -339,6 +308,8 @@ bool accInit(uint32_t targetLooptime)
         acc.extremes[axis].max = -100;
     }
 
+    // At this poinrt acc.dev.accAlign was set up by the driver from the busDev record
+    // If configuration says different - override
     if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
         acc.dev.accAlign = accelerometerConfig()->acc_align;
     }
@@ -347,11 +318,10 @@ bool accInit(uint32_t targetLooptime)
 
 static bool calibratedPosition[6];
 static int32_t accSamples[6][3];
-static int  calibratedAxisCount = 0;
 
 uint8_t accGetCalibrationAxisFlags(void)
 {
-    if (accIsCalibrationComplete() && STATE(ACCELEROMETER_CALIBRATED)) {
+    if (STATE(ACCELEROMETER_CALIBRATED)) {
         return 0x3F;    // All 6 bits are set
     }
 
@@ -390,19 +360,20 @@ static int getPrimaryAxisIndex(int32_t accADCData[3])
         return -1;
 }
 
-bool accIsCalibrationComplete(void)
-{
-    return zeroCalibrationIsCompleteV(&zeroCalibration);
-}
-
 void accStartCalibration(void)
 {
     int positionIndex = getPrimaryAxisIndex(accADC);
 
+    // Fail if we can't detect the side
     if (positionIndex < 0) {
         return;
     }
 
+    // Fail if we have accelerometer fully calibrated and are NOT starting with TOP-UP position
+    if (STATE(ACCELEROMETER_CALIBRATED) && positionIndex != 0) {
+        return;
+    }
+
     // Top+up and first calibration cycle, reset everything
     if (positionIndex == 0) {
         for (int axis = 0; axis < 6; axis++) {
@@ -412,7 +383,6 @@ void accStartCalibration(void)
             accSamples[axis][Z] = 0;
         }
 
-        calibratedAxisCount = 0;
         DISABLE_STATE(ACCELEROMETER_CALIBRATED);
     }
 
@@ -420,8 +390,30 @@ void accStartCalibration(void)
     zeroCalibrationStartV(&zeroCalibration, CALIBRATING_ACC_TIME_MS, acc.dev.acc_1G * 0.05f, true);
 }
 
+static bool allOrientationsHaveCalibrationDataCollected(void)
+{
+    // Return true only if we have calibration data for all 6 positions
+    return calibratedPosition[0] && calibratedPosition[1] && calibratedPosition[2] &&
+           calibratedPosition[3] && calibratedPosition[4] && calibratedPosition[5];
+}
+
+bool accIsCalibrationComplete(void)
+{
+    return zeroCalibrationIsCompleteV(&zeroCalibration);
+}
+
 static void performAcclerationCalibration(void)
 {
+    // Shortcut - no need to do any math if acceleromter is marked as calibrated
+    if (STATE(ACCELEROMETER_CALIBRATED)) {
+        return;
+    }
+
+    // If zero calibration logic is finished - no need to do anything
+    if (accIsCalibrationComplete()) {
+        return;
+    }
+
     fpVector3_t v;
     int positionIndex = getPrimaryAxisIndex(accADC);
 
@@ -446,7 +438,6 @@ static void performAcclerationCalibration(void)
                 accSamples[positionIndex][Z] = v.v[Z];
 
                 calibratedPosition[positionIndex] = true;
-                calibratedAxisCount++;
             }
             else {
                 calibratedPosition[positionIndex] = false;
@@ -456,9 +447,10 @@ static void performAcclerationCalibration(void)
         }
     }
 
-    if (calibratedAxisCount == 6) {
+    if (allOrientationsHaveCalibrationDataCollected()) {
         sensorCalibrationState_t calState;
         float accTmp[3];
+        bool calFailed = false;
 
         /* Calculate offset */
         sensorCalibrationResetState(&calState);
@@ -467,7 +459,12 @@ static void performAcclerationCalibration(void)
             sensorCalibrationPushSampleForOffsetCalculation(&calState, accSamples[axis]);
         }
 
-        sensorCalibrationSolveForOffset(&calState, accTmp);
+        if (!sensorCalibrationSolveForOffset(&calState, accTmp)) {
+            accTmp[0] = 0.0f;
+            accTmp[1] = 0.0f;
+            accTmp[1] = 0.0f;
+            calFailed = true;
+        }
 
         accelerometerConfigMutable()->accZero.raw[X] = lrintf(accTmp[X]);
         accelerometerConfigMutable()->accZero.raw[Y] = lrintf(accTmp[Y]);
@@ -486,13 +483,28 @@ static void performAcclerationCalibration(void)
             sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.dev.acc_1G);
         }
 
-        sensorCalibrationSolveForScale(&calState, accTmp);
+        if (!sensorCalibrationSolveForScale(&calState, accTmp)) {
+            accTmp[0] = 1.0f;
+            accTmp[1] = 1.0f;
+            accTmp[1] = 1.0f;
+            calFailed = true;
+        }
 
         for (int axis = 0; axis < 3; axis++) {
             accelerometerConfigMutable()->accGain.raw[axis] = lrintf(accTmp[axis] * 4096);
         }
 
-        saveConfigAndNotify();
+        if (calFailed) {
+            // If failed - don't save and also invalidate the calibration data for all positions
+            for (int axis = 0; axis < 6; axis++) {
+                calibratedPosition[axis] = false;
+            }
+        }
+        else {
+            // saveConfigAndNotify will trigger eepromREAD and in turn call back the accelerometer gain validation
+            // that will set ENABLE_STATE(ACCELEROMETER_CALIBRATED) if all is good
+            saveConfigAndNotify();
+        }
     }
 }
 
@@ -537,10 +549,7 @@ void accUpdate(void)
         DEBUG_SET(DEBUG_ACC, axis, accADC[axis]);
     }
 
-    if (!accIsCalibrationComplete()) {
-        performAcclerationCalibration();
-        return;
-    }
+    performAcclerationCalibration();
 
     applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
 
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index 872c7aab49..01d2582ded 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -466,7 +466,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
         case CURRENT_SENSOR_VIRTUAL:
             amperage = batteryMetersConfig()->current.offset;
             if (ARMING_FLAG(ARMED)) {
-                throttleStatus_e throttleStatus = calculateThrottleStatus();
+                throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
                 int32_t throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
                 int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
                 amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;
diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c
index b23edb7af7..205f7c8df3 100644
--- a/src/main/sensors/boardalignment.c
+++ b/src/main/sensors/boardalignment.c
@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "common/maths.h"
 #include "common/vector.h"
 #include "common/axis.h"
@@ -85,7 +87,7 @@ void updateBoardAlignment(int16_t roll, int16_t pitch)
     initBoardAlignment();
 }
 
-void FAST_CODE applyBoardAlignment(int32_t *vec)
+void applyBoardAlignment(int32_t *vec)
 {
     if (standardBoardAlignment) {
         return;
diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c
index ca1c4c67f0..e00859a196 100644
--- a/src/main/sensors/diagnostics.c
+++ b/src/main/sensors/diagnostics.c
@@ -30,13 +30,16 @@ extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
 
 hardwareSensorStatus_e getHwGyroStatus(void)
 {
-    // Gyro is assumed to be always healthy
+    // Gyro is assumed to be always healthy, but it must be present
+    if (detectedSensors[SENSOR_INDEX_GYRO] == GYRO_NONE) {
+        return HW_SENSOR_UNAVAILABLE;
+    }
+
     return HW_SENSOR_OK;
 }
 
 hardwareSensorStatus_e getHwAccelerometerStatus(void)
 {
-#if defined(USE_ACC)
     if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
         if (accIsHealthy()) {
             return HW_SENSOR_OK;
@@ -55,9 +58,6 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
             return HW_SENSOR_NONE;
         }
     }
-#else
-    return HW_SENSOR_NONE;
-#endif
 }
 
 hardwareSensorStatus_e getHwCompassStatus(void)
diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c
index 618db67221..f4bde550f1 100644
--- a/src/main/sensors/esc_sensor.c
+++ b/src/main/sensors/esc_sensor.c
@@ -75,14 +75,30 @@ static escSensorData_t  escSensorData[MAX_SUPPORTED_MOTORS];
 static escSensorData_t  escSensorDataCombined;
 static bool             escSensorDataNeedsUpdate;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 1);
 PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig,
-    .currentOffset = 0,
+    .currentOffset = 0, // UNUSED
+    .listenOnly = 0,
 );
 
+static int getTelemetryMotorCount(void)
+{
+    if (escSensorConfig()->listenOnly) {
+        return 1;
+    }
+    else {
+        return getMotorCount();
+    }
+}
+
 static void escSensorSelectNextMotor(void)
 {
-    escSensorMotor = (escSensorMotor + 1) % getMotorCount();
+    if (escSensorConfig()->listenOnly) {
+        escSensorMotor = 0;
+    }
+    else {
+        escSensorMotor = (escSensorMotor + 1) % getTelemetryMotorCount();
+    }
 }
 
 static void escSensorIncreaseDataAge(void)
@@ -130,7 +146,7 @@ static bool escSensorDecodeFrame(void)
     return ESC_SENSOR_FRAME_PENDING;
 }
 
-uint32_t FAST_CODE computeRpm(int16_t erpm) {
+uint32_t computeRpm(int16_t erpm) {
     return lrintf((float)erpm * ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2));
 }
 
@@ -154,7 +170,7 @@ escSensorData_t * escSensorGetData(void)
 
         // Combine data only from active sensors, ignore stale sensors
         int usedEscSensorCount = 0;
-        for (int i = 0; i < getMotorCount(); i++) {
+        for (int i = 0; i < getTelemetryMotorCount(); i++) {
             if (escSensorData[i].dataAge < ESC_DATA_INVALID) {
                 usedEscSensorCount++;
                 escSensorDataCombined.dataAge = MAX(escSensorDataCombined.dataAge, escSensorData[i].dataAge);
@@ -167,7 +183,7 @@ escSensorData_t * escSensorGetData(void)
 
         // Make sure we calculate our sensor values only from non-stale values
         if (usedEscSensorCount) {
-            escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset;
+            escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getTelemetryMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset;
             escSensorDataCombined.voltage = (uint32_t)escSensorDataCombined.voltage / usedEscSensorCount;
             escSensorDataCombined.rpm = (float)escSensorDataCombined.rpm / usedEscSensorCount;
         }
@@ -229,7 +245,9 @@ void escSensorUpdate(timeUs_t currentTimeUs)
             break;
 
         case ESC_SENSOR_READY:
-            pwmRequestMotorTelemetry(escSensorMotor);
+            if (!escSensorConfig()->listenOnly) {
+                pwmRequestMotorTelemetry(escSensorMotor);
+            }
             bufferPosition = 0;
             escTriggerTimeMs = currentTimeMs;
             escSensorState = ESC_SENSOR_WAITING;
@@ -238,6 +256,7 @@ void escSensorUpdate(timeUs_t currentTimeUs)
         case ESC_SENSOR_WAITING:
             if ((currentTimeMs - escTriggerTimeMs) >= ESC_REQUEST_TIMEOUT_MS) {
                 // Timed out. Select next motor and move on
+                escSensorIncreaseDataAge();
                 escSensorSelectNextMotor();
                 escSensorState = ESC_SENSOR_READY;
             }
diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h
index 97d5671d42..e205af981d 100644
--- a/src/main/sensors/esc_sensor.h
+++ b/src/main/sensors/esc_sensor.h
@@ -34,6 +34,7 @@ typedef struct {
 
 typedef struct escSensorConfig_s {
     uint16_t currentOffset;             // offset consumed by the flight controller / VTX / cam / ... in mA
+    uint8_t  listenOnly;
 } escSensorConfig_t;
 
 PG_DECLARE(escSensorConfig_t, escSensorConfig);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 27934c549c..5849833f29 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "build/build_config.h"
 #include "build/debug.h"
 
@@ -77,11 +79,11 @@
 
 FASTRAM gyro_t gyro; // gyro sensor object
 
-STATIC_UNIT_TESTED gyroDev_t gyroDev0;  // Not in FASTRAM since it may hold DMA buffers
-STATIC_FASTRAM int16_t gyroTemperature0;
+#define MAX_GYRO_COUNT          1
 
-STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration;
-STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT];
+STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT];  // Not in FASTRAM since it may hold DMA buffers
+STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT];
+STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration[MAX_GYRO_COUNT];
 
 STATIC_FASTRAM filterApplyFnPtr gyroLpfApplyFn;
 STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT];
@@ -132,115 +134,88 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_AUTODETECT:
         FALLTHROUGH;
 
-#ifdef USE_GYRO_MPU6050
+#ifdef USE_IMU_MPU6050
     case GYRO_MPU6050:
         if (mpu6050GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6050;
-#ifdef GYRO_MPU6050_ALIGN
-            dev->gyroAlign = GYRO_MPU6050_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_L3G4200D
+#ifdef USE_IMU_L3G4200D
     case GYRO_L3G4200D:
         if (l3g4200dDetect(dev)) {
             gyroHardware = GYRO_L3G4200D;
-#ifdef GYRO_L3G4200D_ALIGN
-            dev->gyroAlign = GYRO_L3G4200D_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_MPU3050
+#ifdef USE_IMU_MPU3050
     case GYRO_MPU3050:
         if (mpu3050Detect(dev)) {
             gyroHardware = GYRO_MPU3050;
-#ifdef GYRO_MPU3050_ALIGN
-            dev->gyroAlign = GYRO_MPU3050_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_L3GD20
+#ifdef USE_IMU_L3GD20
     case GYRO_L3GD20:
         if (l3gd20Detect(dev)) {
             gyroHardware = GYRO_L3GD20;
-#ifdef GYRO_L3GD20_ALIGN
-            dev->gyroAlign = GYRO_L3GD20_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_MPU6000
+#ifdef USE_IMU_MPU6000
     case GYRO_MPU6000:
         if (mpu6000GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6000;
-#ifdef GYRO_MPU6000_ALIGN
-            dev->gyroAlign = GYRO_MPU6000_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
 #endif
 
-#if defined(USE_GYRO_MPU6500)
+#if defined(USE_IMU_MPU6500)
     case GYRO_MPU6500:
         if (mpu6500GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6500;
-#ifdef GYRO_MPU6500_ALIGN
-            dev->gyroAlign = GYRO_MPU6500_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_MPU9250
+#ifdef USE_IMU_MPU9250
     case GYRO_MPU9250:
         if (mpu9250GyroDetect(dev)) {
             gyroHardware = GYRO_MPU9250;
-#ifdef GYRO_MPU9250_ALIGN
-            dev->gyroAlign = GYRO_MPU9250_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_BMI160
+#ifdef USE_IMU_BMI160
     case GYRO_BMI160:
         if (bmi160GyroDetect(dev)) {
             gyroHardware = GYRO_BMI160;
-#ifdef GYRO_BMI160_ALIGN
-            dev->gyroAlign = GYRO_BMI160_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_ICM20689
+#ifdef USE_IMU_ICM20689
     case GYRO_ICM20689:
         if (icm20689GyroDetect(dev)) {
             gyroHardware = GYRO_ICM20689;
-#ifdef GYRO_ICM20689_ALIGN
-            dev->gyroAlign = GYRO_ICM20689_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
 #endif
 
-#ifdef USE_FAKE_GYRO
+#ifdef USE_IMU_FAKE
     case GYRO_FAKE:
         if (fakeGyroDetect(dev)) {
             gyroHardware = GYRO_FAKE;
@@ -254,54 +229,9 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         gyroHardware = GYRO_NONE;
     }
 
-    if (gyroHardware != GYRO_NONE) {
-        detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
-        sensorsSet(SENSOR_GYRO);
-    }
     return gyroHardware;
 }
 
-bool gyroInit(void)
-{
-    memset(&gyro, 0, sizeof(gyro));
-
-    // Set inertial sensor tag (for dual-gyro selection)
-#ifdef USE_DUAL_GYRO
-    gyroDev0.imuSensorToUse = gyroConfig()->gyro_to_use;
-#else
-    gyroDev0.imuSensorToUse = 0;
-#endif
-
-    if (gyroDetect(&gyroDev0, GYRO_AUTODETECT) == GYRO_NONE) {
-        return false;
-    }
-
-    // Driver initialisation
-    gyroDev0.lpf = gyroConfig()->gyro_lpf;
-    gyroDev0.requestedSampleIntervalUs = gyroConfig()->looptime;
-    gyroDev0.sampleRateIntervalUs = gyroConfig()->looptime;
-    gyroDev0.initFn(&gyroDev0);
-
-    // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
-    gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev0.sampleRateIntervalUs : gyroConfig()->looptime;
-
-    if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
-        gyroDev0.gyroAlign = gyroConfig()->gyro_align;
-    }
-
-    gyroInitFilters();
-#ifdef USE_DYNAMIC_FILTERS
-    dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
-    gyroDataAnalyseStateInit(
-        &gyroAnalyseState, 
-        gyroConfig()->dynamicGyroNotchMinHz,
-        gyroConfig()->dynamicGyroNotchRange,
-        getLooptime()
-    );
-#endif
-    return true;
-}
-
 static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff)
 {
     *applyFn = nullFilterApply;
@@ -324,7 +254,7 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t
     }
 }
 
-void gyroInitFilters(void)
+static void gyroInitFilters(void)
 {
     STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
     notchFilter1ApplyFn = nullFilterApply;
@@ -352,14 +282,74 @@ void gyroInitFilters(void)
     }
 }
 
-void gyroStartCalibration(void)
+bool gyroInit(void)
 {
-    zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
+    memset(&gyro, 0, sizeof(gyro));
+
+    // Set inertial sensor tag (for dual-gyro selection)
+#ifdef USE_DUAL_GYRO
+    gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use;
+#else
+    gyroDev[0].imuSensorToUse = 0;
+#endif
+
+    // Detecting gyro0
+    gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
+    if (gyroHardware == GYRO_NONE) {
+        gyro.initialized = false;
+        detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE;
+        return true;
+    }
+
+    // Gyro is initialized
+    gyro.initialized = true;
+    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
+    sensorsSet(SENSOR_GYRO);
+
+    // Driver initialisation
+    gyroDev[0].lpf = gyroConfig()->gyro_lpf;
+    gyroDev[0].requestedSampleIntervalUs = gyroConfig()->looptime;
+    gyroDev[0].sampleRateIntervalUs = gyroConfig()->looptime;
+    gyroDev[0].initFn(&gyroDev[0]);
+
+    // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
+    gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime;
+
+    // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record
+    // If configuration says different - override
+    if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
+        gyroDev[0].gyroAlign = gyroConfig()->gyro_align;
+    }
+
+    gyroInitFilters();
+#ifdef USE_DYNAMIC_FILTERS
+    dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
+    gyroDataAnalyseStateInit(
+        &gyroAnalyseState, 
+        gyroConfig()->dynamicGyroNotchMinHz,
+        gyroConfig()->dynamicGyroNotchRange,
+        getLooptime()
+    );
+#endif
+    return true;
 }
 
-bool FAST_CODE NOINLINE gyroIsCalibrationComplete(void)
+void gyroStartCalibration(void)
 {
-    return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration);
+    if (!gyro.initialized) {
+        return;
+    }
+
+    zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
+}
+
+bool gyroIsCalibrationComplete(void)
+{
+    if (!gyro.initialized) {
+        return true;
+    }
+
+    return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]);
 }
 
 STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
@@ -400,33 +390,57 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
     }
 }
 
-void FAST_CODE NOINLINE gyroUpdate()
+static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf)
 {
     // range: +/- 8192; +/- 2000 deg/sec
-    if (gyroDev0.readFn(&gyroDev0)) {
-        if (zeroCalibrationIsCompleteV(&gyroCalibration)) {
+    if (gyroDev->readFn(gyroDev)) {
+        if (zeroCalibrationIsCompleteV(gyroCal)) {
+            int32_t gyroADCtmp[XYZ_AXIS_COUNT];
+
             // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
-            gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
-            gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
-            gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z];
-            applySensorAlignment(gyroADC, gyroADC, gyroDev0.gyroAlign);
-            applyBoardAlignment(gyroADC);
+            gyroADCtmp[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X];
+            gyroADCtmp[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y];
+            gyroADCtmp[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z];
+
+            // Apply sensor alignment
+            applySensorAlignment(gyroADCtmp, gyroADCtmp, gyroDev->gyroAlign);
+            applyBoardAlignment(gyroADCtmp);
+
+            // Convert to deg/s and store in unified data
+            gyroADCf[X] = (float)gyroADCtmp[X] * gyroDev->scale;
+            gyroADCf[Y] = (float)gyroADCtmp[Y] * gyroDev->scale;
+            gyroADCf[Z] = (float)gyroADCtmp[Z] * gyroDev->scale;
+
+            return true;
         } else {
-            performGyroCalibration(&gyroDev0, &gyroCalibration);
+            performGyroCalibration(gyroDev, gyroCal);
+
             // Reset gyro values to zero to prevent other code from using uncalibrated data
-            gyro.gyroADCf[X] = 0.0f;
-            gyro.gyroADCf[Y] = 0.0f;
-            gyro.gyroADCf[Z] = 0.0f;
-            // still calibrating, so no need to further process gyro data
-            return;
+            gyroADCf[X] = 0.0f;
+            gyroADCf[Y] = 0.0f;
+            gyroADCf[Z] = 0.0f;
+
+            return false;
         }
     } else {
         // no gyro reading to process
+        return false;
+    }
+}
+
+void FAST_CODE NOINLINE gyroUpdate()
+{
+    if (!gyro.initialized) {
+        return;
+    }
+
+    if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) {
         return;
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale;
+        // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s]
+        float gyroADCf = gyro.gyroADCf[axis];
 
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
 
@@ -470,9 +484,13 @@ void FAST_CODE NOINLINE gyroUpdate()
 
 bool gyroReadTemperature(void)
 {
+    if (!gyro.initialized) {
+        return false;
+    }
+
     // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10]
-    if (gyroDev0.temperatureFn) {
-        return gyroDev0.temperatureFn(&gyroDev0, &gyroTemperature0);
+    if (gyroDev[0].temperatureFn) {
+        return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]);
     }
 
     return false;
@@ -480,18 +498,31 @@ bool gyroReadTemperature(void)
 
 int16_t gyroGetTemperature(void)
 {
-    return gyroTemperature0;
+    if (!gyro.initialized) {
+        return 0;
+    }
+
+    return gyroTemperature[0];
 }
 
 int16_t gyroRateDps(int axis)
 {
-    return lrintf(gyro.gyroADCf[axis] / gyroDev0.scale);
+    if (!gyro.initialized) {
+        return 0;
+    }
+
+    return lrintf(gyro.gyroADCf[axis]);
 }
 
 bool gyroSyncCheckUpdate(void)
 {
-    if (!gyroDev0.intStatusFn)
+    if (!gyro.initialized) {
         return false;
+    }
 
-    return gyroDev0.intStatusFn(&gyroDev0);
+    if (!gyroDev[0].intStatusFn) {
+        return false;
+    }
+
+    return gyroDev[0].intStatusFn(&gyroDev[0]);
 }
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 51e7fda3af..414d9042cc 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -50,6 +50,7 @@ typedef enum {
 #define DYN_NOTCH_RANGE_HZ_LOW 1000
 
 typedef struct gyro_s {
+    bool initialized;
     uint32_t targetLooptime;
     float gyroADCf[XYZ_AXIS_COUNT];
 } gyro_t;
@@ -80,7 +81,6 @@ typedef struct gyroConfig_s {
 PG_DECLARE(gyroConfig_t, gyroConfig);
 
 bool gyroInit(void);
-void gyroInitFilters(void);
 void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF);
 void gyroUpdate(void);
 void gyroStartCalibration(void);
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 394c627c69..274b76eb86 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -27,16 +27,17 @@
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
-#include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
-#include "sensors/pitotmeter.h"
-#include "sensors/gyro.h"
 #include "sensors/compass.h"
-#include "sensors/rangefinder.h"
-#include "sensors/opflow.h"
-#include "sensors/temperature.h"
+#include "sensors/gyro.h"
 #include "sensors/initialisation.h"
+#include "sensors/irlock.h"
+#include "sensors/opflow.h"
+#include "sensors/pitotmeter.h"
+#include "sensors/rangefinder.h"
+#include "sensors/sensors.h"
+#include "sensors/temperature.h"
 
 uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
@@ -102,6 +103,10 @@ bool sensorsAutodetect(void)
     }
 #endif
 
+#ifdef USE_IRLOCK
+    irlockInit();
+#endif
+
     if (eepromUpdatePending) {
         writeEEPROM();
     }
diff --git a/src/main/sensors/irlock.c b/src/main/sensors/irlock.c
new file mode 100644
index 0000000000..186ae3d10f
--- /dev/null
+++ b/src/main/sensors/irlock.c
@@ -0,0 +1,136 @@
+/*
+ * 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 "stdbool.h"
+#include "stdint.h"
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/log.h"
+#include "common/printf.h"
+
+#include "drivers/irlock.h"
+#include "drivers/time.h"
+
+#include "fc/runtime_config.h"
+
+#include "flight/imu.h"
+
+#include "navigation/navigation.h"
+
+#include "sensors/sensors.h"
+#include "sensors/barometer.h"
+#include "sensors/irlock.h"
+
+
+#define IRLOCK_TIMEOUT 100
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+enum {
+    DEBUG_IRLOCK_DETECTED,
+    DEBUG_IRLOCK_MEAS_VALID,
+    DEBUG_IRLOCK_POS_X,
+    DEBUG_IRLOCK_POS_Y
+};
+
+static irlockDev_t irlockDev;
+static bool irlockDetected = false;
+static bool measurementValid = false;
+static irlockData_t irlockData;
+static timeMs_t lastUpdateMs = 0;
+
+void irlockInit(void)
+{
+    irlockDetected = irlockDetect(&irlockDev);
+    DEBUG_SET(DEBUG_IRLOCK, DEBUG_IRLOCK_DETECTED, irlockDetected);
+}
+
+bool irlockHasBeenDetected(void)
+{
+    return irlockDetected;
+}
+
+bool irlockMeasurementIsValid(void) {
+    return measurementValid;
+}
+
+void irlockUpdate(void)
+{
+    if (irlockDetected && irlockDev.read(&irlockDev, &irlockData)) lastUpdateMs = millis();
+    measurementValid = millis() - lastUpdateMs < IRLOCK_TIMEOUT;
+
+    if (debugMode == DEBUG_IRLOCK) {
+        float distX, distY;
+        bool valid = irlockGetPosition(&distX, &distY);
+        debug[DEBUG_IRLOCK_MEAS_VALID] = valid;
+        debug[DEBUG_IRLOCK_POS_X] = lrintf(distX * 100);
+        debug[DEBUG_IRLOCK_POS_Y] = lrintf(distY * 100);
+    }
+}
+
+#define X_TO_DISTANCE_FACTOR -0.0029387573f
+#define X_TO_DISTANCE_OFFSET 0.4702011635f
+#define Y_TO_DISTANCE_FACTOR -0.0030568431f
+#define Y_TO_DISTANCE_OFFSET 0.3056843086f
+
+#define LENS_X_CORRECTION 4.4301355e-6f
+#define LENS_Y_CORRECTION 4.7933139e-6f
+
+// calculate distance relative to center at 1 meter distance from absolute object coordinates and taking into account lens distortion
+static void irlockCoordinatesToDistance(uint16_t pixX, uint16_t pixY, float *distX, float *distY)
+{
+    int16_t xCenterOffset = pixX - IRLOCK_RES_X / 2;
+    int16_t yCenterOffset = pixY - IRLOCK_RES_Y / 2;
+    float lensDistortionCorrectionFactor = LENS_X_CORRECTION * xCenterOffset * xCenterOffset + LENS_Y_CORRECTION * yCenterOffset * yCenterOffset - 1.0f;
+    *distX = (X_TO_DISTANCE_FACTOR * pixX + X_TO_DISTANCE_OFFSET) / lensDistortionCorrectionFactor;
+    *distY = (Y_TO_DISTANCE_FACTOR * pixY + Y_TO_DISTANCE_OFFSET) / lensDistortionCorrectionFactor;
+}
+
+bool irlockGetPosition(float *distX, float *distY)
+{
+    if (!measurementValid) return false;
+
+    // calculate edges of the object
+    int16_t corner1X = irlockData.posX - irlockData.sizeX / 2;
+    int16_t corner1Y = irlockData.posY - irlockData.sizeY / 2;
+    int16_t corner2X = irlockData.posX + irlockData.sizeX / 2;
+    int16_t corner2Y = irlockData.posY + irlockData.sizeY / 2;
+
+    // convert pixel position to distance
+    float corner1DistX, corner1DistY, corner2DistX, corner2DistY;
+    irlockCoordinatesToDistance(corner1X, corner1Y, &corner1DistX, &corner1DistY);
+    irlockCoordinatesToDistance(corner2X, corner2Y, &corner2DistX, &corner2DistY);
+
+    // return center of object
+    float uDistX = (corner1DistX + corner2DistX) / 2.0f; // lateral movement, positive means the aircraft is to the left of the beacon
+    float uDistY = (corner1DistY + corner2DistY) / 2.0f; // longitudinal movement, positive means the aircraft is in front of the beacon
+
+    // compensate for altitude and attitude
+    float altitude = CENTIMETERS_TO_METERS(getEstimatedActualPosition(Z));
+    *distX = altitude * tan_approx(atan2_approx(uDistX, 1.0f) - DECIDEGREES_TO_RADIANS(attitude.values.roll));
+    *distY = altitude * tan_approx(atan2_approx(uDistY, 1.0f) + DECIDEGREES_TO_RADIANS(attitude.values.pitch));
+
+    return true;
+}
+
+#endif /* USE_IRLOCK */
diff --git a/src/main/sensors/irlock.h b/src/main/sensors/irlock.h
new file mode 100644
index 0000000000..03270b6c5c
--- /dev/null
+++ b/src/main/sensors/irlock.h
@@ -0,0 +1,29 @@
+/*
+ * 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
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+void irlockInit(void);
+bool irlockHasBeenDetected(void);
+
+void irlockUpdate(void);
+bool irlockMeasurementIsValid(void);
+bool irlockGetPosition(float *distX, float *distY);
+
+#endif /* USE_IRLOCK */
diff --git a/src/main/startup/startup_stm32f30x_md_gcc.S b/src/main/startup/startup_stm32f30x_md_gcc.S
index 466d2a7add..819c92734f 100644
--- a/src/main/startup/startup_stm32f30x_md_gcc.S
+++ b/src/main/startup/startup_stm32f30x_md_gcc.S
@@ -70,12 +70,11 @@ defined in linker script */
   .weak  Reset_Handler
   .type  Reset_Handler, %function
 Reset_Handler:
-  ldr r0, =0x20009FFC         // HJI 11/9/2012
-  ldr r1, =0xDEADBEEF         // HJI 11/9/2012
-  ldr r2, [r0, #0]            // HJI 11/9/2012
-  str r0, [r0, #0]            // HJI 11/9/2012
-  cmp r2, r1                  // HJI 11/9/2012
-  beq Reboot_Loader           // HJI 11/9/2012
+
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
+
 
 /* Copy the data segment initializers from flash to SRAM */
   movs  r1, #0
diff --git a/src/main/startup/startup_stm32f40xx.s b/src/main/startup/startup_stm32f40xx.s
index 764bc499cb..9e3a33932d 100644
--- a/src/main/startup/startup_stm32f40xx.s
+++ b/src/main/startup/startup_stm32f40xx.s
@@ -80,13 +80,9 @@ Reset_Handler:
   str     r1, [r0, #0x30]
   dsb
 
-  // Check for bootloader reboot
-  ldr r0, =0x2001FFFC         // mj666
-  ldr r1, =0xDEADBEEF         // mj666
-  ldr r2, [r0, #0]            // mj666
-  str r0, [r0, #0]            // mj666
-  cmp r2, r1                  // mj666
-  beq Reboot_Loader           // mj666
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
 
 /* Copy the data segment initializers from flash to SRAM */ 
   movs  r1, #0
@@ -158,13 +154,6 @@ LoopMarkHeapStack:
 LoopForever:
   b LoopForever
 
-Reboot_Loader:                // mj666
-  // Reboot to ROM            // mj666
-  ldr     r0, =0x1FFF0000     // mj666
-  ldr     sp,[r0, #0]         // mj666
-  ldr     r0,[r0, #4]         // mj666
-  bx      r0                  // mj666
-
 .size  Reset_Handler, .-Reset_Handler
 
 /**
diff --git a/src/main/startup/startup_stm32f411xe.s b/src/main/startup/startup_stm32f411xe.s
index bf174f25ba..06fb51a84d 100644
--- a/src/main/startup/startup_stm32f411xe.s
+++ b/src/main/startup/startup_stm32f411xe.s
@@ -80,13 +80,9 @@ Reset_Handler:
   str     r1, [r0, #0x30]
   dsb
 
-  // Check for bootloader reboot
-  ldr r0, =0x2001FFFC         // mj666
-  ldr r1, =0xDEADBEEF         // mj666
-  ldr r2, [r0, #0]            // mj666
-  str r0, [r0, #0]            // mj666
-  cmp r2, r1                  // mj666
-  beq Reboot_Loader           // mj666
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
 
 /* Copy the data segment initializers from flash to SRAM */ 
   movs  r1, #0
diff --git a/src/main/startup/startup_stm32f427xx.s b/src/main/startup/startup_stm32f427xx.s
index 520a7f7222..064f187fd6 100755
--- a/src/main/startup/startup_stm32f427xx.s
+++ b/src/main/startup/startup_stm32f427xx.s
@@ -87,13 +87,9 @@ Reset_Handler:
   str     r1, [r0, #0x30]
   dsb
 
-  // Check for bootloader reboot
-  ldr r0, =0x2001FFFC         // mj666
-  ldr r1, =0xDEADBEEF         // mj666
-  ldr r2, [r0, #0]            // mj666
-  str r0, [r0, #0]            // mj666
-  cmp r2, r1                  // mj666
-  beq Reboot_Loader           // mj666
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
 
 /* Copy the data segment initializers from flash to SRAM */ 
   movs  r1, #0
@@ -165,25 +161,6 @@ LoopMarkHeapStack:
 LoopForever:
   b LoopForever
 
-Reboot_Loader:                // mj666
-  // RCC->APB2ENR |= RCC_APB2Periph_SYSCFG;
-  ldr     r0, =0x40023800
-  ldr     r1, [r0, #0x44]
-  orr     r1, r1, 0x00004000    // RCC_APB2Periph_SYSCFG
-  str     r1, [r0, #0x44]
-
-  // Remap system memory to 0x00000000
-  // SYSCFG->MEMRMP = SYSCFG_MemoryRemap_SystemFlash
-  ldr     r0, =0x40013800
-  ldr     r1, =0x00000001
-  str     r1, [r0]
-
-  // Reboot to ROM            // mj666
-  ldr     r0, =0x1FFF0000     // mj666
-  ldr     sp,[r0, #0]         // mj666
-  ldr     r0,[r0, #4]         // mj666
-  bx      r0                  // mj666
-
 .size  Reset_Handler, .-Reset_Handler
 
 /**
diff --git a/src/main/startup/startup_stm32f446xx.s b/src/main/startup/startup_stm32f446xx.s
index 592a03de67..4686636823 100644
--- a/src/main/startup/startup_stm32f446xx.s
+++ b/src/main/startup/startup_stm32f446xx.s
@@ -82,13 +82,9 @@ Reset_Handler:
   str     r1, [r0, #0x30]
   dsb
 
-  // Check for bootloader reboot
-  ldr r0, =0x2001FFFC         // mj666
-  ldr r1, =0xDEADBEEF         // mj666
-  ldr r2, [r0, #0]            // mj666
-  str r0, [r0, #0]            // mj666
-  cmp r2, r1                  // mj666
-  beq Reboot_Loader           // mj666
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
 
 /* Copy the data segment initializers from flash to SRAM */
   movs  r1, #0
diff --git a/src/main/startup/startup_stm32f722xx.s b/src/main/startup/startup_stm32f722xx.s
index f856d04e3b..2e8d766626 100644
--- a/src/main/startup/startup_stm32f722xx.s
+++ b/src/main/startup/startup_stm32f722xx.s
@@ -79,6 +79,8 @@ defined in linker script */
 Reset_Handler:  
   ldr   sp, =_estack      /* set stack pointer */
 
+  bl persistentObjectInit
+
 /* Copy the data segment initializers from flash to SRAM */  
   movs  r1, #0
   b  LoopCopyDataInit
diff --git a/src/main/startup/startup_stm32f745xx.s b/src/main/startup/startup_stm32f745xx.s
index eb6aa25659..48b4a79657 100644
--- a/src/main/startup/startup_stm32f745xx.s
+++ b/src/main/startup/startup_stm32f745xx.s
@@ -77,6 +77,10 @@ defined in linker script */
   .weak  Reset_Handler
   .type  Reset_Handler, %function
 Reset_Handler:  
+  ldr   sp, =_estack      /* set stack pointer */
+
+  bl persistentObjectInit
+
 /* Copy the data segment initializers from flash to SRAM */  
   movs  r1, #0
   b  LoopCopyDataInit
diff --git a/src/main/startup/startup_stm32f746xx.s b/src/main/startup/startup_stm32f746xx.s
index 8ef14a877f..47d4c6841b 100644
--- a/src/main/startup/startup_stm32f746xx.s
+++ b/src/main/startup/startup_stm32f746xx.s
@@ -79,6 +79,8 @@ defined in linker script */
 Reset_Handler:  
   ldr   sp, =_estack      /* set stack pointer */
 
+  bl persistentObjectInit
+
 /* Copy the data segment initializers from flash to SRAM */  
   movs  r1, #0
   b  LoopCopyDataInit
diff --git a/src/main/startup/startup_stm32f765xx.s b/src/main/startup/startup_stm32f765xx.s
index 29c9ac1a28..84d4b30e9c 100644
--- a/src/main/startup/startup_stm32f765xx.s
+++ b/src/main/startup/startup_stm32f765xx.s
@@ -77,6 +77,10 @@ defined in linker script */
   .weak  Reset_Handler
   .type  Reset_Handler, %function
 Reset_Handler:  
+  ldr   sp, =_estack      /* set stack pointer */
+
+  bl persistentObjectInit
+
 /* Copy the data segment initializers from flash to SRAM */  
   movs  r1, #0
   b  LoopCopyDataInit
diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h
index 2ab53fca08..6e77fde6ad 100644
--- a/src/main/target/AIKONF4/target.h
+++ b/src/main/target/AIKONF4/target.h
@@ -31,21 +31,15 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW0_DEG
+#define MPU6000_CS_PIN          SPI1_NSS_PIN
+#define MPU6000_SPI_BUS         BUS_SPI1
 
-#define MPU6000_CS_PIN           SPI1_NSS_PIN
-#define MPU6000_SPI_BUS          BUS_SPI1
-#define GYRO_MPU6000_ALIGN       CW0_DEG
-
-#define MPU6500_CS_PIN          MPU6000_CS_PIN
-#define MPU6500_SPI_BUS         MPU6000_SPI_BUS
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
+#define MPU6500_CS_PIN          SPI1_NSS_PIN
+#define MPU6500_SPI_BUS         BUS_SPI1
 
 #define SERIAL_PORT_COUNT       7   //VCP, UART1, UART2, UART3, UART4, SOFTSERIAL1, SOFTSERIAL2
 
diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h
index 85c4fcbd98..edf6973b54 100644
--- a/src/main/target/AIRBOTF4/target.h
+++ b/src/main/target/AIRBOTF4/target.h
@@ -34,24 +34,16 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
-
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
 #define MAG_HMC5883_ALIGN       CW90_DEG
diff --git a/src/main/target/AIRBOTF7/OMNIBUSF7NANOV7.mk b/src/main/target/AIRBOTF7/OMNIBUSF7NANOV7.mk
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/main/target/AIRBOTF7/target.c b/src/main/target/AIRBOTF7/target.c
new file mode 100644
index 0000000000..a24923a5cb
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target.c
@@ -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/.
+ */
+
+#include <stdint.h>
+
+#include <platform.h>
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/bus.h"
+#include "drivers/sensor.h"
+
+// Board hardware definitions
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6000,    DEVHW_MPU6000,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE,   GYRO_0_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6500,    DEVHW_MPU6500,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE,   GYRO_0_ALIGN);
+
+// OMNIBUSF7NANOV7 doesn't have a second gyro
+#ifndef OMNIBUSF7NANOV7
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6000,    DEVHW_MPU6000,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE,   GYRO_1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6500,    DEVHW_MPU6500,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE,   GYRO_1_ALIGN);
+#endif
+
+const timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_LED,              0, 0), // LED
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_ANY,              0, 0), // Cam control, SS, UNUSED
+
+    DEF_TIM(TIM8,  CH3, PC8,  TIM_USE_MC_MOTOR,         0, 0), //S1
+    DEF_TIM(TIM4,  CH1, PB6,  TIM_USE_MC_MOTOR,         0, 0), //S2
+    DEF_TIM(TIM8,  CH4, PC9,  TIM_USE_MC_MOTOR,         0, 0), //S3
+    DEF_TIM(TIM4,  CH2, PB7,  TIM_USE_MC_MOTOR,         0, 0), //S4
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/AIRBOTF7/target.h b/src/main/target/AIRBOTF7/target.h
new file mode 100644
index 0000000000..8b4a267710
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target.h
@@ -0,0 +1,142 @@
+/*
+ * 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
+
+// ************** Beeper and LED ******************
+#define LED0                    PB12
+#define BEEPER                  PB0
+#define BEEPER_INVERTED
+
+// *************** SPI & I2C **********************
+#define USE_SPI
+#define USE_I2C
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN           PB4
+#define SPI3_MOSI_PIN           PB5
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9 
+
+#ifdef OMNIBUSF7NANOV7
+#   include "target_o7v7.h"
+#else
+#   include "target_abf7.h"
+#endif
+
+// *************** OSD *****************************
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          PC15
+
+// *************** I2C defvices **********************
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#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
+
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_RX_PIN            PA10
+#define UART1_TX_PIN            PA9
+
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            NONE
+
+#define USE_UART3
+#define UART3_RX_PIN            PC11
+#define UART3_TX_PIN            PC10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART5
+#define UART5_TX_PIN            PC12
+#define UART5_RX_PIN            NONE
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_TX_PIN     PA5
+#define SOFTSERIAL_1_RX_PIN     PA5
+
+#define SERIAL_PORT_COUNT       8
+
+// ************** Other features *******************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA15
+
+#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
+
+#define DEFAULT_RX_TYPE             RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER           SERIALRX_SBUS
+#define SERIALRX_UART               SERIAL_PORT_USART1
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define MAX_PWM_OUTPUT_PORTS        4
+#define TARGET_MOTOR_COUNT          4
+
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+#define TARGET_IO_PORTD         0xffff
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR
diff --git a/src/main/target/AIRBOTF7/target.mk b/src/main/target/AIRBOTF7/target.mk
new file mode 100644
index 0000000000..24cc94224d
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target.mk
@@ -0,0 +1,16 @@
+F7X2RE_TARGETS += $(TARGET)
+FEATURES       += VCP ONBOARDFLASH MSC
+
+TARGET_SRC = \
+            drivers/accgyro/accgyro_mpu6500.c \
+            drivers/accgyro/accgyro_mpu6000.c \
+            drivers/barometer/barometer_bmp280.c \
+            drivers/barometer/barometer_ms56xx.c \
+            drivers/compass/compass_hmc5883l.c \
+            drivers/compass/compass_mag3110.c \
+            drivers/compass/compass_qmc5883l.c \
+            drivers/compass/compass_ist8310.c \
+            drivers/compass/compass_ist8308.c \
+            drivers/compass/compass_lis3mdl.c \
+            drivers/light_ws2811strip.c \
+            drivers/max7456.c
diff --git a/src/main/target/AIRBOTF7/target_abf7.h b/src/main/target/AIRBOTF7/target_abf7.h
new file mode 100644
index 0000000000..861d04f493
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target_abf7.h
@@ -0,0 +1,72 @@
+/*
+ * 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
+
+#define TARGET_BOARD_IDENTIFIER "ABF7"
+#define USBD_PRODUCT_STRING     "Airbot F7"
+
+
+// *************** Gyro & ACC **********************
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+#define USE_DUAL_GYRO
+
+#define USE_IMU_MPU6000
+#define USE_IMU_MPU6500
+
+#define GYRO_0_CS_PIN           PD2
+#define GYRO_0_SPI_BUS          BUS_SPI3
+#define GYRO_0_EXTI_PIN         NONE
+#define GYRO_0_ALIGN            CW90_DEG
+
+#define GYRO_1_CS_PIN           PC4
+#define GYRO_1_SPI_BUS          BUS_SPI1
+#define GYRO_1_EXTI_PIN         NONE
+#define GYRO_1_ALIGN            CW0_DEG
+
+// *************** FLASH **************************
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PA3
+#define M25P16_SPI_BUS          BUS_SPI1
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** UART *****************************
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            NONE
+
+#define SERIAL_PORT_COUNT       8
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_3
+
diff --git a/src/main/target/AIRBOTF7/target_o7v7.h b/src/main/target/AIRBOTF7/target_o7v7.h
new file mode 100644
index 0000000000..c808ef6cdb
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target_o7v7.h
@@ -0,0 +1,64 @@
+/*
+ * 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
+
+#define TARGET_BOARD_IDENTIFIER "O7V7"
+#define USBD_PRODUCT_STRING     "OmnibusF7 Nano V7"
+
+
+// *************** Gyro & ACC **********************
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+
+#define USE_IMU_MPU6000
+#define USE_IMU_MPU6500
+
+#define GYRO_0_CS_PIN           PD2
+#define GYRO_0_SPI_BUS          BUS_SPI3
+#define GYRO_0_EXTI_PIN         NONE
+#define GYRO_0_ALIGN            CW0_DEG
+
+// *************** FLASH **************************
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PA2
+#define M25P16_SPI_BUS          BUS_SPI1
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** UART *****************************
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            NONE
+
+#define SERIAL_PORT_COUNT       8
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#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
+
diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h
index 53f1a52acf..9356bf0cb2 100755
--- a/src/main/target/AIRHEROF3/target.h
+++ b/src/main/target/AIRHEROF3/target.h
@@ -34,20 +34,14 @@
 #define USE_SPI
 #define USE_SPI_DEVICE_2
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
 #define MPU6500_SPI_BUS         BUS_SPI2
 #define MPU6500_CS_PIN          PB12
 
 #define BMP280_SPI_BUS          BUS_SPI2
 #define BMP280_CS_PIN           PB5
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW0_DEG
-
 #define USE_BARO
 #define USE_BARO_BMP280
 
diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h
index 128364d1eb..ba8de96eea 100644
--- a/src/main/target/ALIENFLIGHTF3/target.h
+++ b/src/main/target/ALIENFLIGHTF3/target.h
@@ -38,28 +38,20 @@
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 // Using MPU6050 for the moment.
-#define USE_GYRO
-
-#define USE_GYRO_MPU6050
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 #define MPU6050_I2C_BUS         BUS_I2C2
-#define GYRO_MPU6050_ALIGN      CW270_DEG
 
-#define USE_GYRO_MPU9250
-#define GYRO_MPU6500_ALIGN      CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 #define MPU6500_CS_PIN          PA15
 #define MPU6500_SPI_BUS         BUS_SPI3
-#define GYRO_MPU9250_ALIGN      CW270_DEG
+
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 #define MPU9250_CS_PIN          PA15
 #define MPU9250_SPI_BUS         BUS_SPI3
 
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW270_DEG
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
-
 // No baro support.
 //#define USE_BARO
 //#define USE_BARO_MS5611
diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h
index 39bb130d0c..0e375b2652 100644
--- a/src/main/target/ALIENFLIGHTF4/target.h
+++ b/src/main/target/ALIENFLIGHTF4/target.h
@@ -45,17 +45,11 @@
 #define MPU9250_CS_PIN          SPI1_NSS_PIN
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h
index 116cf92d5f..197d8a5875 100644
--- a/src/main/target/ALIENFLIGHTNGF7/target.h
+++ b/src/main/target/ALIENFLIGHTNGF7/target.h
@@ -42,17 +42,11 @@
 #define MPU9250_CS_PIN          SPI1_NSS_PIN
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h
index 6959a3856b..78d1cab751 100644
--- a/src/main/target/ANYFC/target.h
+++ b/src/main/target/ANYFC/target.h
@@ -30,13 +30,8 @@
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 
 // MPU6000 interrupts
 #define USE_EXTI
diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h
index 288c8136f2..16baaca9cc 100644
--- a/src/main/target/ANYFCF7/target.h
+++ b/src/main/target/ANYFCF7/target.h
@@ -30,13 +30,8 @@
 #define MPU6000_CS_PIN        PA4
 #define MPU6000_SPI_BUS       BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN CW270_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN CW270_DEG
 
 // MPU6000 interrupts
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h
index c591f5691b..ca42a0ad2e 100644
--- a/src/main/target/ANYFCM7/target.h
+++ b/src/main/target/ANYFCM7/target.h
@@ -30,13 +30,8 @@
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN CW270_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN CW270_DEG
 
 // MPU6000 interrupts
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/ASGARD32F4/target.h b/src/main/target/ASGARD32F4/target.h
index 6351de47d4..49c8dd10c3 100644
--- a/src/main/target/ASGARD32F4/target.h
+++ b/src/main/target/ASGARD32F4/target.h
@@ -39,15 +39,10 @@
 // #define GYRO_INT_EXTI            PC8
 // #define USE_MPU_DATA_READY_SIGNAL        // Not connected on FireworksV2
 
-#define USE_GYRO
-#define USE_ACC
-
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-#define ACC_MPU6000_ALIGN       CW90_DEG
 
 // #define USE_MAG
 // #define MAG_I2C_BUS             BUS_I2C2
diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h
index 76c58145a7..f26650eeeb 100644
--- a/src/main/target/ASGARD32F7/target.h
+++ b/src/main/target/ASGARD32F7/target.h
@@ -39,15 +39,10 @@
 // #define GYRO_INT_EXTI            PC8
 // #define USE_MPU_DATA_READY_SIGNAL        // Not connected on FireworksV2
 
-#define USE_GYRO
-#define USE_ACC
-
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-#define ACC_MPU6000_ALIGN       CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h
index 1c11634ba8..7f7ed3dea6 100644
--- a/src/main/target/BEEROTORF4/target.h
+++ b/src/main/target/BEEROTORF4/target.h
@@ -36,13 +36,8 @@
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h
index 3b70e722fc..2f50dc866e 100755
--- a/src/main/target/BETAFLIGHTF3/target.h
+++ b/src/main/target/BETAFLIGHTF3/target.h
@@ -26,13 +26,8 @@
 #define MPU6000_CS_PIN          PA15
 #define MPU6000_SPI_BUS    BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 
 // MPU6000 interrupts
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h
index 0fa8b8beec..cf4727c7b3 100755
--- a/src/main/target/BETAFLIGHTF4/target.h
+++ b/src/main/target/BETAFLIGHTF4/target.h
@@ -30,14 +30,8 @@
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS		BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 
 
 // MPU6000 interrupts
diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c
index f49c88c932..adfb5a81a5 100644
--- a/src/main/target/BLUEJAYF4/hardware_revision.c
+++ b/src/main/target/BLUEJAYF4/hardware_revision.c
@@ -84,7 +84,7 @@ void detectHardwareRevision(void)
 }
 
 /* BJF4_REV1 has different connection of memory chip */
-BUSDEV_REGISTER_SPI_TAG(m25p16_bjf3_rev1, DEVHW_M25P16, M25P16_SPI_BUS, PB3, NONE, 1, DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(m25p16_bjf3_rev1, DEVHW_M25P16, M25P16_SPI_BUS, PB3, NONE, 1, DEVFLAGS_NONE, 0);
 
 void updateHardwareRevision(void)
 {
@@ -93,7 +93,7 @@ void updateHardwareRevision(void)
     }
 
     /* if flash exists on PB3 (busDevice m25p16_bjf3_rev1) then Rev1 */
-    if (m25p16_init(1)) {
+    if (flashInit()) {
         hardwareRevision = BJF4_REV1;
     } else {
         IOInit(IOGetByTag(IO_TAG(PB3)), OWNER_FREE, RESOURCE_NONE, 0);
diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h
index 92184c0930..06cca4cf11 100644
--- a/src/main/target/BLUEJAYF4/target.h
+++ b/src/main/target/BLUEJAYF4/target.h
@@ -40,13 +40,8 @@
 #define MPU6500_CS_PIN          PC4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW0_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW0_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h
index 3fdacef844..ad03b71bcb 100644
--- a/src/main/target/CHEBUZZF3/target.h
+++ b/src/main/target/CHEBUZZF3/target.h
@@ -29,22 +29,17 @@
 #define USE_SPI
 #define USE_SPI_DEVICE_1
 
-#define USE_GYRO
-#define USE_GYRO_L3GD20
-#define USE_GYRO_MPU6050
+#define USE_IMU_L3GD20
+#define USE_IMU_MPU6050
+#define USE_IMU_LSM303DLHC
 
 #define MPU6050_I2C_BUS                 BUS_I2C1
 #define LSM303DLHC_I2C_BUS              BUS_I2C1
 #define L3GD20_SPI_BUS                  BUS_SPI1
 #define L3GD20_CS_PIN                   PE3
 
-#define GYRO_L3GD20_ALIGN CW270_DEG
-#define GYRO_MPU6050_ALIGN CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define USE_ACC_LSM303DLHC
-#define ACC_MPU6050_ALIGN       CW0_DEG
+#define IMU_L3GD20_ALIGN       CW270_DEG
+#define IMU_MPU6050_ALIGN      CW0_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h
index 54955f2910..992e7e79c4 100644
--- a/src/main/target/CLRACINGF4AIR/target.h
+++ b/src/main/target/CLRACINGF4AIR/target.h
@@ -43,15 +43,13 @@
 #else
 #define SPI3_MOSI_PIN           PC12
 #endif
+
 //MPU-9250
 #define MPU9250_CS_PIN          PA4
 #define MPU9250_SPI_BUS         BUS_SPI1
-#define USE_GYRO
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW0_DEG
-#define USE_ACC
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW0_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW0_DEG
+
 #define USE_MAG
 #define USE_MAG_MPU9250
 #define MAG_MPU9250_ALIGN       CW90_DEG
diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h
index 53d6dd0be5..d852c19a06 100755
--- a/src/main/target/COLIBRI/target.h
+++ b/src/main/target/COLIBRI/target.h
@@ -39,11 +39,7 @@
 #define MPU6000_CS_PIN          PC4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-
-#define USE_GYRO
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
 
 // MPU6000 interrupts
 #define USE_EXTI
@@ -62,12 +58,10 @@
 #define TEMPERATURE_I2C_BUS     BUS_I2C3
 
 #ifdef QUANTON
-#define ACC_MPU6000_ALIGN       CW90_DEG
-#define GYRO_MPU6000_ALIGN      CW90_DEG
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MAG_HMC5883_ALIGN       CW90_DEG
 #else
-#define ACC_MPU6000_ALIGN       CW270_DEG_FLIP
-#define GYRO_MPU6000_ALIGN      CW270_DEG_FLIP
+#define IMU_MPU6000_ALIGN       CW270_DEG_FLIP
 #define MAG_HMC5883_ALIGN       CW270_DEG_FLIP
 #endif
 
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index 82cc819192..57e6a98eee 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/src/main/target/COLIBRI_RACE/target.h
@@ -52,21 +52,12 @@
 #define MPU9250_CS_PIN          SPI1_NSS_PIN
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C2
diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h
index 538aac3394..b478f8e238 100644
--- a/src/main/target/DALRCF405/target.h
+++ b/src/main/target/DALRCF405/target.h
@@ -36,22 +36,15 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_ACC
-
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW90_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW90_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 
 
 //FLASH
diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h
index 124069874f..534042b8da 100644
--- a/src/main/target/DALRCF722DUAL/target.h
+++ b/src/main/target/DALRCF722DUAL/target.h
@@ -26,16 +26,11 @@
 #define BEEPER                  PC13
 #define BEEPER_INVERTED
 
-#define USE_GYRO
-#define USE_ACC
-
 // MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PB0
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
 #define USE_EXTI
 #define GYRO_INT_EXTI            PB10
@@ -143,6 +138,9 @@
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
 #define MAX_PWM_OUTPUT_PORTS    6
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR
 
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff
diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h
index 9231b8a808..6c2ebc7925 100644
--- a/src/main/target/F4BY/target.h
+++ b/src/main/target/F4BY/target.h
@@ -39,19 +39,11 @@
 #define ICM20689_SPI_BUS        BUS_SPI1
 
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW90_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-
-#define USE_ACC_ICM20689
-#define ACC_ICM20689_ALIGN       CW90_DEG
-
-#define USE_GYRO_ICM20689
-#define GYRO_ICM20689_ALIGN      CW90_DEG
+#define USE_IMU_ICM20689
+#define IMU_ICM20689_ALIGN       CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
diff --git a/src/main/target/FALCORE/target.h b/src/main/target/FALCORE/target.h
index 8e370c958e..d8480eda73 100755
--- a/src/main/target/FALCORE/target.h
+++ b/src/main/target/FALCORE/target.h
@@ -35,13 +35,8 @@
 #define GYRO_INT_EXTI            PB0
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN CW0_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN CW0_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h
index dec7d460e6..df981eeb99 100644
--- a/src/main/target/FF_F35_LIGHTNING/target.h
+++ b/src/main/target/FF_F35_LIGHTNING/target.h
@@ -35,13 +35,8 @@
 #define MPU9250_CS_PIN          PC0
 #define MPU9250_SPI_BUS         BUS_SPI3
 
-#define USE_ACC
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW180_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW180_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG
 
 #define USE_MAG
 #define USE_MAG_MPU9250
diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h
index 44a87b5e69..669f4bd440 100644
--- a/src/main/target/FF_FORTINIF4/target.h
+++ b/src/main/target/FF_FORTINIF4/target.h
@@ -45,13 +45,8 @@
 #define MPU6500_CS_PIN          PA8
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 /*---------------------------------*/
 
 /*------------FLASH----------------*/
diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h
index 5ec9a8f634..59337ad53b 100644
--- a/src/main/target/FF_PIKOF4/target.h
+++ b/src/main/target/FF_PIKOF4/target.h
@@ -64,19 +64,11 @@
 #define MPU6500_SPI_BUS         BUS_SPI1
 #endif
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
-
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 /*---------------------------------*/
 
 #if defined(FF_PIKOF4OSD)
diff --git a/src/main/target/FIREWORKSV2/target.c b/src/main/target/FIREWORKSV2/target.c
index 1eb592444d..3866b61fc0 100644
--- a/src/main/target/FIREWORKSV2/target.c
+++ b/src/main/target/FIREWORKSV2/target.c
@@ -29,11 +29,19 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 #include "drivers/pwm_output.h"
 #include "common/maths.h"
 #include "fc/config.h"
 
+// Board hardware definitions - IMU1 slot
+BUSDEV_REGISTER_SPI_TAG(busdev_1_mpu6000,   DEVHW_MPU6000,  IMU_1_SPI_BUS,  IMU_1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU_1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_1_mpu6500,   DEVHW_MPU6500,  IMU_1_SPI_BUS,  IMU_1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU_1_ALIGN);
+
+// Board hardware definitions - IMU2 slot
+BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6000,   DEVHW_MPU6000,  IMU_2_SPI_BUS,  IMU_2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU_2_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6500,   DEVHW_MPU6500,  IMU_2_SPI_BUS,  IMU_2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU_2_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM,                           0, 0), // PPM
diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h
index c11eeda584..b8dd5c6ef6 100644
--- a/src/main/target/FIREWORKSV2/target.h
+++ b/src/main/target/FIREWORKSV2/target.h
@@ -62,32 +62,28 @@
 #define GYRO_INT_EXTI            PC8
 // #define USE_MPU_DATA_READY_SIGNAL        // Not connected on FireworksV2
 
-#define USE_GYRO
-#define USE_ACC
-
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
+#define USE_DUAL_GYRO
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS     // Don't use common busdev descriptors for IMU
+#define USE_IMU_MPU6500
+#define USE_IMU_MPU6000
 
 #if defined(OMNIBUSF4V6)
-#define MPU6500_CS_PIN          PC14
-#define MPU6500_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-#define ACC_MPU6500_ALIGN       CW0_DEG
+#   define IMU_1_CS_PIN            PA4
+#   define IMU_1_SPI_BUS           BUS_SPI1
+#   define IMU_1_ALIGN             CW180_DEG
+#   define IMU_2_CS_PIN            PC14
+#   define IMU_2_SPI_BUS           BUS_SPI1
+#   define IMU_2_ALIGN             CW0_DEG
 #else
-#define MPU6500_CS_PIN          PD2
-#define MPU6500_SPI_BUS         BUS_SPI3
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define ACC_MPU6500_ALIGN       CW180_DEG
+    // FIREWORKS V2
+#   define IMU_1_CS_PIN            PD2
+#   define IMU_1_SPI_BUS           BUS_SPI3
+#   define IMU_1_ALIGN             CW180_DEG
+#   define IMU_2_CS_PIN            PA4
+#   define IMU_2_SPI_BUS           BUS_SPI1
+#   define IMU_2_ALIGN             CW0_DEG_FLIP
 #endif
 
-// OmnibusF4 Nano v6 and OmnibusF4 V6 has a MPU6000
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
-#define MPU6000_CS_PIN          PA4
-#define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define ACC_MPU6000_ALIGN       CW180_DEG
-
 #define USE_MAG
 #if defined(OMNIBUSF4V6)
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h
index bbea161301..91fd29491b 100644
--- a/src/main/target/FISHDRONEF4/target.h
+++ b/src/main/target/FISHDRONEF4/target.h
@@ -38,17 +38,11 @@
 #define MPU9250_CS_PIN          PA4
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define USE_ACC_MPU9250
-#define GYRO_MPU9250_ALIGN      CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-#define USE_GYRO_MPU9250
-#define ACC_MPU9250_ALIGN       CW180_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG
 
 // MPU6500 interrupts
 #define USE_EXTI
diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h
index dc8b964c7e..c4c38aa3c7 100644
--- a/src/main/target/FOXEERF405/target.h
+++ b/src/main/target/FOXEERF405/target.h
@@ -27,27 +27,21 @@
 
 /*** IMU sensors ***/
 #define USE_EXTI
-#define USE_GYRO
-#define USE_ACC
 
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
 // MPU6000
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-#define ACC_MPU6000_ALIGN       CW90_DEG
 
 // ICM20689 - handled by MPU6500 driver
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 #define MPU6500_CS_PIN          PB2
 #define MPU6500_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
 
 /*** SPI/I2C bus ***/
 #define USE_SPI
diff --git a/src/main/target/FOXEERF722DUAL/target.c b/src/main/target/FOXEERF722DUAL/target.c
index 27d080d663..80285de5a5 100644
--- a/src/main/target/FOXEERF722DUAL/target.c
+++ b/src/main/target/FOXEERF722DUAL/target.c
@@ -20,10 +20,11 @@
 #include "drivers/io.h"
 #include "drivers/bus.h"
 #include "drivers/timer.h"
+#include "drivers/sensor.h"
 #include "drivers/pwm_mapping.h"
 
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4,  CH2, PB7,  TIM_USE_PPM,                          0, 0),  // PPM&SBUS  
diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h
index a770e605b5..75eec081ff 100644
--- a/src/main/target/FOXEERF722DUAL/target.h
+++ b/src/main/target/FOXEERF722DUAL/target.h
@@ -27,8 +27,6 @@
 
 /*** IMU sensors ***/
 #define USE_EXTI
-#define USE_ACC
-#define USE_GYRO
 
 // We use dual IMU sensors, they have to be described in the target file
 #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
@@ -36,24 +34,18 @@
 #define USE_DUAL_GYRO
 
 // MPU6000
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
-
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
 #define MPU6000_EXTI_PIN        PC4
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-#define ACC_MPU6000_ALIGN       CW270_DEG
 
 // ICM20602 - handled by MPU6500 driver
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
-
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 #define MPU6500_CS_PIN          PB1
 #define MPU6500_SPI_BUS         BUS_SPI1
 #define MPU6500_EXTI_PIN        PB0
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define ACC_MPU6500_ALIGN       CW180_DEG
 
 /*** SPI/I2C bus ***/
 #define USE_SPI
diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h
index 6fe5507157..b046c399cb 100644
--- a/src/main/target/FRSKYF3/target.h
+++ b/src/main/target/FRSKYF3/target.h
@@ -28,16 +28,10 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define MPU_ADDRESS             0x69
 
-#define USE_GYRO
-#define USE_ACC
-
 #define MPU6050_I2C_BUS         BUS_I2C1
 
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW270_DEG
-
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW270_DEG
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 
 #define USE_VCP
 #define USE_UART1
diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h
index b727701ace..4d950a18a7 100755
--- a/src/main/target/FRSKYF4/target.h
+++ b/src/main/target/FRSKYF4/target.h
@@ -25,13 +25,8 @@
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 
 #define USE_EXTI
 #define GYRO_INT_EXTI            PC4
diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h
index 114ee507c3..9091457131 100755
--- a/src/main/target/FURYF3/target.h
+++ b/src/main/target/FURYF3/target.h
@@ -29,31 +29,21 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG  // changedkb 270
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG  // changedkb 270
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW90_DEG  // changedkb 270
 #define MPU9250_CS_PIN          PA4
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG  // changedkb 270
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW90_DEG  // changedkb 270
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW90_DEG  // changedkb 270
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG  // changedkb 270
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW90_DEG  // changedkb 270
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW90_DEG  // changedkb 270
-
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
 #define USE_MAG_MPU9250
diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h
index d380ac7bed..c4603911cb 100644
--- a/src/main/target/FURYF4OSD/target.h
+++ b/src/main/target/FURYF4OSD/target.h
@@ -35,9 +35,13 @@
 #define USE_SPI
 #define USE_SPI_DEVICE_1
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
@@ -45,20 +49,6 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
-
 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
 #define USE_SPI_DEVICE_3
 #define SPI3_SCK_PIN            PC10
diff --git a/src/main/target/IFLIGHTF4_TWING/target.c b/src/main/target/IFLIGHTF4_TWING/target.c
index d6566965b0..b4f212ddd6 100644
--- a/src/main/target/IFLIGHTF4_TWING/target.c
+++ b/src/main/target/IFLIGHTF4_TWING/target.c
@@ -23,10 +23,11 @@
 #include "drivers/io.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
+#include "drivers/sensor.h"
 
 // Board hardware definitions
-BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6500,  MPU6500_0_SPI_BUS,  MPU6500_0_CS_PIN,   MPU6500_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,  MPU6500_1_SPI_BUS,  MPU6500_1_CS_PIN,   MPU6500_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6500,  MPU6500_0_SPI_BUS,  MPU6500_0_CS_PIN,   MPU6500_0_EXTI_PIN,     0,  DEVFLAGS_NONE,  IMU_0_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,  MPU6500_1_SPI_BUS,  MPU6500_1_CS_PIN,   MPU6500_1_EXTI_PIN,     1,  DEVFLAGS_NONE,  IMU_1_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM
diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h
index 6f5d6521bb..27d99bdcf7 100644
--- a/src/main/target/IFLIGHTF4_TWING/target.h
+++ b/src/main/target/IFLIGHTF4_TWING/target.h
@@ -34,12 +34,11 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_ACC
-#define USE_GYRO 
 #define USE_DUAL_GYRO
 
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_0_ALIGN             CW0_DEG
+#define IMU_1_ALIGN             CW0_DEG
 
 #define MPU6500_0_CS_PIN        PA4
 #define MPU6500_0_SPI_BUS       BUS_SPI1
@@ -49,9 +48,6 @@
 #define MPU6500_1_SPI_BUS       BUS_SPI1
 #define MPU6500_1_EXTI_PIN      PA8
 
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-#define ACC_MPU6500_ALIGN       CW0_DEG
-
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
 
diff --git a/src/main/target/IFLIGHTF7_TWING/target.c b/src/main/target/IFLIGHTF7_TWING/target.c
index 46285e6857..83090b5c94 100644
--- a/src/main/target/IFLIGHTF7_TWING/target.c
+++ b/src/main/target/IFLIGHTF7_TWING/target.c
@@ -23,10 +23,11 @@
 #include "drivers/io.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
+#include "drivers/sensor.h"
 
 // Board hardware definitions
-BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6500,  MPU6500_0_SPI_BUS,  MPU6500_0_CS_PIN,   MPU6500_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,  MPU6500_1_SPI_BUS,  MPU6500_1_CS_PIN,   MPU6500_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6500,  MPU6500_0_SPI_BUS,  MPU6500_0_CS_PIN,   MPU6500_0_EXTI_PIN,     0,  DEVFLAGS_NONE,  IMU_0_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,  MPU6500_1_SPI_BUS,  MPU6500_1_CS_PIN,   MPU6500_1_EXTI_PIN,     1,  DEVFLAGS_NONE,  IMU_1_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM,                         0, 0),
diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h
index 7741e9617a..a6a9e63de2 100644
--- a/src/main/target/IFLIGHTF7_TWING/target.h
+++ b/src/main/target/IFLIGHTF7_TWING/target.h
@@ -36,12 +36,11 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_ACC
-#define USE_GYRO 
 #define USE_DUAL_GYRO
 
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_0_ALIGN       CW90_DEG
+#define IMU_1_ALIGN       CW0_DEG
 
 #define MPU6500_0_CS_PIN        PC3
 #define MPU6500_0_SPI_BUS       BUS_SPI1
@@ -51,8 +50,6 @@
 #define MPU6500_1_SPI_BUS       BUS_SPI1
 #define MPU6500_1_EXTI_PIN      PA8
 
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
 
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
@@ -158,4 +155,4 @@
 #define MAX_PWM_OUTPUT_PORTS        8
 
 #define USE_DSHOT
-#define USE_ESC_SENSOR
\ No newline at end of file
+#define USE_ESC_SENSOR
diff --git a/src/main/target/KAKUTEF4/target.c b/src/main/target/KAKUTEF4/target.c
index 48082d288a..469c0d63b0 100755
--- a/src/main/target/KAKUTEF4/target.c
+++ b/src/main/target/KAKUTEF4/target.c
@@ -31,10 +31,10 @@
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM,   0, 0), // PPM IN
 
-    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT - DMA1_ST7
-    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT - DMA1_ST2
-    DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT - DMA1_ST6
-    DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - DMA1_ST1
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR |                    TIM_USE_FW_MOTOR, 0, 0), // S1_OUT - DMA1_ST7
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR |                    TIM_USE_FW_MOTOR, 0, 0), // S2_OUT - DMA1_ST2
+    DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO, 0, 1), // S3_OUT - DMA1_ST6
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - DMA1_ST1
 
 #if defined(KAKUTEF4V2)
     DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED,   0, 0), // LED_STRIP - DMA2_ST2
diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h
index ed79614f08..0bc68d3e3d 100755
--- a/src/main/target/KAKUTEF4/target.h
+++ b/src/main/target/KAKUTEF4/target.h
@@ -42,16 +42,11 @@
 #define GYRO_INT_EXTI            PC5
 #define USE_MPU_DATA_READY_SIGNAL
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 #define MPU6500_CS_PIN          PC4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
 
 #ifdef KAKUTEF4V2
 #   define USE_I2C
@@ -169,7 +164,9 @@
 #define TARGET_IO_PORTC 0xffff
 #define TARGET_IO_PORTD        (BIT(2))
 
-
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR
 
 #ifdef KAKUTEF4V2
 #   define MAX_PWM_OUTPUT_PORTS       4
diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h
index 82be9590a0..4e4c81aa97 100644
--- a/src/main/target/KAKUTEF7/target.h
+++ b/src/main/target/KAKUTEF7/target.h
@@ -36,30 +36,22 @@
 #define USE_DSHOT
 #define USE_ESC_SENSOR
 
-#define USE_ACC
-#define USE_GYRO
-
 #define USE_MPU_DATA_READY_SIGNAL
 #define USE_EXTI
 
 // ICM-20689
-#define USE_ACC_ICM20689
-#define USE_GYRO_ICM20689
-#define GYRO_ICM20689_ALIGN      CW270_DEG
-#define ACC_ICM20689_ALIGN       CW270_DEG
-
+#define USE_IMU_ICM20689
+#define IMU_ICM20689_ALIGN       CW270_DEG
 #define GYRO_INT_EXTI            PE1
 #define ICM20689_CS_PIN          SPI4_NSS_PIN
 #define ICM20689_SPI_BUS         BUS_SPI4
 
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define GYRO_INT_EXTI           PE1
-#define GYRO_MPU6000_ALIGN      CW270_DEG
 #define MPU6000_CS_PIN          SPI4_NSS_PIN
 #define MPU6000_SPI_BUS         BUS_SPI4
 
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
 
 #define USB_IO
 #define USE_VCP
@@ -156,6 +148,8 @@
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
 
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
 #define USE_ADC
 #define ADC_CHANNEL_1_PIN           PC2
 #define ADC_CHANNEL_2_PIN           PC3
diff --git a/src/main/target/KFC32F3_INAV/target.h b/src/main/target/KFC32F3_INAV/target.h
index 40808156df..26e6ebeecb 100755
--- a/src/main/target/KFC32F3_INAV/target.h
+++ b/src/main/target/KFC32F3_INAV/target.h
@@ -36,16 +36,11 @@
 
 #define BUS_SPI_SPEED_MAX BUS_SPEED_SLOW
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_CS_PIN          PB5
 #define MPU6000_SPI_BUS         BUS_SPI2
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN CW90_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h
index bf37520717..a97d2b6166 100644
--- a/src/main/target/KISSFC/target.h
+++ b/src/main/target/KISSFC/target.h
@@ -29,15 +29,10 @@
 #define BEEPER              PB13
 #define BEEPER_INVERTED
 
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW180_DEG
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW180_DEG
 #define MPU6050_I2C_BUS         BUS_I2C1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW180_DEG
-
 #define USE_EXTI
 #define MPU_INT_EXTI            PB2
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h
index 924b619277..d0b86ce00c 100755
--- a/src/main/target/KROOZX/target.h
+++ b/src/main/target/KROOZX/target.h
@@ -36,13 +36,8 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define GYRO_INT_EXTI            PA4
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW90_DEG_FLIP
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW90_DEG_FLIP
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG_FLIP
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h
index ca13846a86..5c64316a83 100644
--- a/src/main/target/LUX_RACE/target.h
+++ b/src/main/target/LUX_RACE/target.h
@@ -42,23 +42,16 @@
 #define SPI1_MOSI_PIN           PB5
 #define SPI1_NSS_PIN            PA4
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
 
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 #define MPU9250_CS_PIN          SPI1_NSS_PIN
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
 
 #define USE_VCP
 #define USE_UART1
diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h
index 67b45f39e4..f4312c06fc 100644
--- a/src/main/target/MAMBAF405US/target.h
+++ b/src/main/target/MAMBAF405US/target.h
@@ -33,20 +33,14 @@
 
 // ******* GYRO and ACC ********
 #define USE_EXTI
-#define USE_GYRO_EXTI
 #define GYRO_INT_EXTI                   PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN               CW180_DEG
 #define MPU6000_SPI_BUS                 BUS_SPI1
 #define MPU6000_CS_PIN                  PA4
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN              CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6500_ALIGN               CW180_DEG
 
 // *************** Baro **************************
 #define USE_I2C
diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h
index 09c3074756..aa324c685c 100644
--- a/src/main/target/MAMBAF722/target.h
+++ b/src/main/target/MAMBAF722/target.h
@@ -33,20 +33,14 @@
 
 // ******* GYRO and ACC ********
 #define USE_EXTI
-#define USE_GYRO_EXTI
 #define GYRO_INT_EXTI                   PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
 #define MPU6000_CS_PIN                  SPI1_NSS_PIN
 #define MPU6000_SPI_BUS                 BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN              CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN               CW180_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN               CW180_DEG
 
 // *************** Baro **************************
 #define USE_I2C
diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h
index df11ef86f7..077155b1ff 100644
--- a/src/main/target/MATEKF405/target.h
+++ b/src/main/target/MATEKF405/target.h
@@ -45,19 +45,11 @@
 #define GYRO_INT_EXTI            PC3
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 
 // *************** SD Card **************************
 #define USE_SDCARD
diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h
index 9c9cd393a0..513b14a8b2 100644
--- a/src/main/target/MATEKF405SE/target.h
+++ b/src/main/target/MATEKF405SE/target.h
@@ -33,6 +33,8 @@
 #define SPI1_MISO_PIN   	    PA6
 #define SPI1_MOSI_PIN   	    PA7
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
@@ -40,13 +42,6 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
 
 // *************** I2C /Baro/Mag *********************
 #define USE_I2C
diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h
index 2a9bcbe83d..7063d18c38 100755
--- a/src/main/target/MATEKF411/target.h
+++ b/src/main/target/MATEKF411/target.h
@@ -35,27 +35,20 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define ACC_MPU6500_ALIGN       CW180_DEG
 
 #define USE_EXTI
 #define GYRO_INT_EXTI            PA1
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_ACC
-
 // *************** SPI2 OSD *****************************
 #define USE_SPI_DEVICE_2
 #define SPI2_SCK_PIN            PB13
diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h
index 9a0a3c5ef1..cbffcd1a2c 100755
--- a/src/main/target/MATEKF411SE/target.h
+++ b/src/main/target/MATEKF411SE/target.h
@@ -34,6 +34,8 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
@@ -41,13 +43,6 @@
 #define GYRO_INT_EXTI            PA14
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
 // *************** SPI2 OSD *****************************
 #define USE_SPI_DEVICE_2
diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h
index 51ef9cdf90..62227998e8 100755
--- a/src/main/target/MATEKF722/target.h
+++ b/src/main/target/MATEKF722/target.h
@@ -35,6 +35,8 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 #define MPU6500_CS_PIN          PC2
 #define MPU6500_SPI_BUS         BUS_SPI1
 
@@ -42,13 +44,6 @@
 #define GYRO_INT_EXTI            PC3
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
 
 // *************** I2C/Baro/Mag *********************
 #define USE_I2C
diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h
index 9c0be7255b..38e9a8f380 100755
--- a/src/main/target/MATEKF722PX/target.h
+++ b/src/main/target/MATEKF722PX/target.h
@@ -34,19 +34,14 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
+#define GYRO_INT_EXTI           PC4
 
-#define USE_ACC_MPU6000
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG_FLIP
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define MPU6000_EXTI_PIN        PC4
-
-#define GYRO_MPU6000_ALIGN      CW180_DEG_FLIP
-#define ACC_MPU6000_ALIGN       CW180_DEG_FLIP
 
 // *************** I2C /Baro/Mag *********************
 #define USE_I2C
@@ -88,11 +83,6 @@
 #define M25P16_CS_PIN           PB12
 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
 
-#define USE_SDCARD
-#define USE_SDCARD_SPI
-#define SDCARD_SPI_BUS          BUS_SPI2
-#define SDCARD_CS_PIN           PC15
-
 // *************** UART *****************************
 #define USE_VCP
 #define USB_DETECT_PIN          PC14
@@ -124,7 +114,7 @@
      
 #define USE_SOFTSERIAL1
 #define SOFTSERIAL_1_TX_PIN      PA2 //TX2 pad
-#define SOFTSERIAL_1_RX_PIN      PA2
+#define SOFTSERIAL_1_RX_PIN      NONE
 
 #define SERIAL_PORT_COUNT       8
 
@@ -173,5 +163,3 @@
 #define USE_SERIALSHOT
 #define USE_ESC_SENSOR
 
-//#define USE_CAMERA_CONTROL
-//#define CAMERA_CONTROL_PIN         	PB15
diff --git a/src/main/target/MATEKF722PX/target.mk b/src/main/target/MATEKF722PX/target.mk
index 5c061571fb..0db5e55e78 100755
--- a/src/main/target/MATEKF722PX/target.mk
+++ b/src/main/target/MATEKF722PX/target.mk
@@ -1,5 +1,5 @@
 F7X2RE_TARGETS += $(TARGET)
-FEATURES       += SDCARD VCP ONBOARDFLASH MSC
+FEATURES       += VCP ONBOARDFLASH MSC
 
 TARGET_SRC = \
             drivers/accgyro/accgyro_mpu6000.c \
diff --git a/src/main/target/MATEKF722SE/target.c b/src/main/target/MATEKF722SE/target.c
index d792cec4bf..2c4170a1ce 100644
--- a/src/main/target/MATEKF722SE/target.c
+++ b/src/main/target/MATEKF722SE/target.c
@@ -24,9 +24,10 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/pinio.h"
+#include "drivers/sensor.h"
 
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH1, PB4,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S1   UP1-2   D(1, 4, 5)
diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h
index 28169a3ab1..56dcdcc886 100644
--- a/src/main/target/MATEKF722SE/target.h
+++ b/src/main/target/MATEKF722SE/target.h
@@ -41,27 +41,21 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_DUAL_GYRO
 
-#define USE_ACC_MPU6000
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG_FLIP
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
 #define MPU6000_EXTI_PIN        PC4
 
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 #define MPU6500_CS_PIN          PC15
 #define MPU6500_SPI_BUS         BUS_SPI1
 #define MPU6500_EXTI_PIN        PC3
 
-#define GYRO_MPU6000_ALIGN      CW180_DEG_FLIP
-#define ACC_MPU6000_ALIGN       CW180_DEG_FLIP
 
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
 
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/MATEKF765/target.c b/src/main/target/MATEKF765/target.c
index 72455ac600..af157cad89 100644
--- a/src/main/target/MATEKF765/target.c
+++ b/src/main/target/MATEKF765/target.c
@@ -24,9 +24,10 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/pinio.h"
+#include "drivers/sensor.h"
 
-BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM2,  CH1, PA0,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S1  UP(1,7), D(1,5,3)
diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h
index 5329c419ae..fcddab2693 100644
--- a/src/main/target/MATEKF765/target.h
+++ b/src/main/target/MATEKF765/target.h
@@ -42,27 +42,20 @@
 #define SPI3_MISO_PIN           PB4
 #define SPI3_MOSI_PIN           PB5
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_DUAL_GYRO
 
-#define USE_ACC_MPU6000
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG_FLIP
 #define MPU6000_SPI_BUS         BUS_SPI1
 #define MPU6000_CS_PIN          PC4
 #define MPU6000_EXTI_PIN        PB2
 
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG_FLIP
 #define MPU6500_SPI_BUS         BUS_SPI3
 #define MPU6500_CS_PIN          PD7
 #define MPU6500_EXTI_PIN        PD4
 
-#define GYRO_MPU6000_ALIGN      CW90_DEG_FLIP
-#define ACC_MPU6000_ALIGN       CW90_DEG_FLIP
-
-#define GYRO_MPU6500_ALIGN      CW270_DEG_FLIP
-#define ACC_MPU6500_ALIGN       CW270_DEG_FLIP
 
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h
index 278bd9654d..386eadf120 100644
--- a/src/main/target/MOTOLAB/target.h
+++ b/src/main/target/MOTOLAB/target.h
@@ -30,18 +30,11 @@
 #define GYRO_INT_EXTI            PA15
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW180_DEG
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW180_DEG
 
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW180_DEG
-
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 
 #define MPU6050_I2C_BUS         BUS_I2C2
 #define MPU6000_CS_PIN          PB12
diff --git a/src/main/target/NOX/target.h b/src/main/target/NOX/target.h
index 7951a76183..9a40ea5050 100755
--- a/src/main/target/NOX/target.h
+++ b/src/main/target/NOX/target.h
@@ -43,6 +43,8 @@
 
 
 // *************** SPI Gyro & ACC **********************
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW0_DEG
 #define MPU6000_CS_PIN          PB12
 #define MPU6000_SPI_BUS         BUS_SPI2
 
@@ -50,11 +52,6 @@
 #define GYRO_INT_EXTI           PA8
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-
-#define USE_ACC
-#define USE_ACC_MPU6000
 
 // *************** SPI BARO *****************************
 #define USE_BARO
diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h
index 1127030379..cdd1b8d33a 100644
--- a/src/main/target/OMNIBUS/target.h
+++ b/src/main/target/OMNIBUS/target.h
@@ -19,8 +19,6 @@
 
 #define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
 
-#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
-
 #define BEEPER                  PC15
 #define BEEPER_INVERTED
 
@@ -32,19 +30,17 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
+#undef USE_VTX_SMARTAUDIO       // Disabled due to flash size
+
 #define USE_EXTI
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_SPI_BUS         BUS_SPI1
 #define MPU6000_CS_PIN          PA4
 #define GYRO_INT_EXTI            PC13
 #define USE_MPU_DATA_READY_SIGNAL
-#define GYRO_MPU6000_ALIGN      CW90_DEG
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW90_DEG
 
 #define USE_BARO
 #define USE_BARO_BMP280
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 991fb6c4ab..0c3666a9d3 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -69,36 +69,23 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_ACC
-
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
 #if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
-  #define USE_GYRO_MPU6000
-  #define GYRO_MPU6000_ALIGN      CW270_DEG
-
-  #define USE_ACC_MPU6000
-  #define ACC_MPU6000_ALIGN       CW270_DEG
+  #define USE_IMU_MPU6000
+  #define IMU_MPU6000_ALIGN       CW270_DEG
 #else
-  #define USE_GYRO_MPU6000
-  #define GYRO_MPU6000_ALIGN      CW180_DEG
-
-  #define USE_ACC_MPU6000
-  #define ACC_MPU6000_ALIGN       CW180_DEG
+  #define USE_IMU_MPU6000
+  #define IMU_MPU6000_ALIGN       CW180_DEG
 #endif
 
 // Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000
 #if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
   #define MPU6500_CS_PIN          MPU6000_CS_PIN
   #define MPU6500_SPI_BUS         MPU6000_SPI_BUS
-
-  #define USE_GYRO_MPU6500
-  #define GYRO_MPU6500_ALIGN      GYRO_MPU6000_ALIGN
-
-  #define USE_ACC_MPU6500
-  #define ACC_MPU6500_ALIGN       ACC_MPU6000_ALIGN
+  #define USE_IMU_MPU6500
+  #define IMU_MPU6500_ALIGN       IMU_MPU6000_ALIGN
 #endif
 
 #define USE_MAG
@@ -140,7 +127,9 @@
 #define VBUS_SENSING_PIN        PC5
 #define VBUS_SENSING_ENABLED
 
+#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
 #define USE_UART_INVERTER
+#endif
 
 #define USE_UART1
 #define UART1_RX_PIN            PA10
diff --git a/src/main/target/OMNIBUSF7/target.c b/src/main/target/OMNIBUSF7/target.c
index 3132b03ac6..ffa46dad9c 100644
--- a/src/main/target/OMNIBUSF7/target.c
+++ b/src/main/target/OMNIBUSF7/target.c
@@ -20,18 +20,19 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 /* GYRO */
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000,  MPU6000_SPI_BUS,    MPU6000_CS_PIN, MPU6000_EXTI_PIN,   0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500,  MPU6500_SPI_BUS,    MPU6500_CS_PIN, MPU6500_EXTI_PIN,   1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
-BUSDEV_REGISTER_SPI(    busdev_bmp280,      DEVHW_BMP280,       BMP280_SPI_BUS,     BMP280_CS_PIN,      NONE,                       DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI(    busdev_bmp280,  DEVHW_BMP280,   BMP280_SPI_BUS,     BMP280_CS_PIN,  NONE,                   DEVFLAGS_NONE,  0);
 
-BUSDEV_REGISTER_I2C(    busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS,        0x1E,               NONE,                       DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C(    busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS,        0x0D,               NONE,                       DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C(    busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS,        0x0E,               NONE,                       DEVFLAGS_NONE);
+BUSDEV_REGISTER_I2C(    busdev_hmc5883, DEVHW_HMC5883,  MAG_I2C_BUS,        0x1E,           NONE,                   DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C(    busdev_qmc5883, DEVHW_QMC5883,  MAG_I2C_BUS,        0x0D,           NONE,                   DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C(    busdev_mag3110, DEVHW_MAG3110,  MAG_I2C_BUS,        0x0E,           NONE,                   DEVFLAGS_NONE,  0);
 
-BUSDEV_REGISTER_SPI(    busdev_max7456,     DEVHW_MAX7456,      MAX7456_SPI_BUS,    MAX7456_CS_PIN,     NONE,                       DEVFLAGS_USE_RAW_REGISTERS);
+BUSDEV_REGISTER_SPI(    busdev_max7456, DEVHW_MAX7456,  MAX7456_SPI_BUS,    MAX7456_CS_PIN, NONE,                   DEVFLAGS_USE_RAW_REGISTERS,  0);
 
 const timerHardware_t timerHardware[] = {
 
diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h
index c3a7262078..d013a36a6c 100644
--- a/src/main/target/OMNIBUSF7/target.h
+++ b/src/main/target/OMNIBUSF7/target.h
@@ -31,47 +31,36 @@
 #define BEEPER   PD15
 #define BEEPER_INVERTED
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_DUAL_GYRO
 
 // ICM-20608-G
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
 
 // MPU6000
-#define USE_ACC_MPU6000
-#define USE_GYRO_MPU6000
 
 #ifdef OMNIBUSF7V2
+#   define USE_IMU_MPU6000
+#   define IMU_MPU6000_ALIGN        CW0_DEG
 #   define MPU6000_CS_PIN           SPI1_NSS_PIN
 #   define MPU6000_SPI_BUS          BUS_SPI1
 #   define MPU6000_EXTI_PIN         PE8
 
+#   define USE_IMU_MPU6500
+#   define IMU_MPU6500_ALIGN        CW90_DEG
 #   define MPU6500_CS_PIN           SPI3_NSS_PIN
 #   define MPU6500_SPI_BUS          BUS_SPI3
 #   define MPU6500_EXTI_PIN         PD0
-// #   define GYRO_1_CS_PIN           MPU6500_CS_PIN
-// #   define GYRO_0_CS_PIN           MPU6000_CS_PIN
-// #   define GYRO_1_INT_EXTI         PD0
-// #   define GYRO_0_INT_EXTI         PE8
-#   define GYRO_MPU6500_ALIGN       CW90_DEG
-#   define ACC_MPU6500_ALIGN        CW90_DEG
 #else
+#   define USE_IMU_MPU6000
+#   define IMU_MPU6000_ALIGN        CW0_DEG
 #   define MPU6000_CS_PIN           SPI3_NSS_PIN
 #   define MPU6000_SPI_BUS          BUS_SPI3
 #   define MPU6000_EXTI_PIN         PD0
 
+#   define USE_IMU_MPU6500
+#   define IMU_MPU6500_ALIGN        CW0_DEG
 #   define MPU6500_CS_PIN           SPI1_NSS_PIN
 #   define MPU6500_SPI_BUS          BUS_SPI1
 #   define MPU6500_EXTI_PIN         PE8
-
-// #   define GYRO_0_CS_PIN           MPU6000_CS_PIN
-// #   define GYRO_1_CS_PIN           MPU6500_CS_PIN
-// #   define GYRO_0_INT_EXTI         PD0
-// #   define GYRO_1_INT_EXTI         PE8
-#   define GYRO_MPU6000_ALIGN      CW0_DEG
-#   define ACC_MPU6000_ALIGN       CW0_DEG
 #endif
 
 #define USE_EXTI
@@ -202,6 +191,8 @@
 // Number of available PWM outputs
 #define MAX_PWM_OUTPUT_PORTS    4
 #define TARGET_MOTOR_COUNT      4
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define TARGET_IO_PORTA 0xffff
 #define TARGET_IO_PORTB 0xffff
@@ -209,4 +200,4 @@
 #define TARGET_IO_PORTD 0xffff
 #define TARGET_IO_PORTE 0xffff
 
-#define PCA9685_I2C_BUS         BUS_I2C2
\ No newline at end of file
+#define PCA9685_I2C_BUS         BUS_I2C2
diff --git a/src/main/target/OMNIBUSF7NXT/target.c b/src/main/target/OMNIBUSF7NXT/target.c
index e6439d4ac1..0222914f0e 100644
--- a/src/main/target/OMNIBUSF7NXT/target.c
+++ b/src/main/target/OMNIBUSF7NXT/target.c
@@ -29,10 +29,11 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 // Board hardware definitions
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     NONE,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     NONE,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     NONE,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     NONE,       1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM,                            0, 0), // PPM / UART1_RX
diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h
index 21257e9e86..b14e0c219e 100644
--- a/src/main/target/OMNIBUSF7NXT/target.h
+++ b/src/main/target/OMNIBUSF7NXT/target.h
@@ -37,24 +37,18 @@
 #define BEEPER                  PC13
 #define BEEPER_INVERTED
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_DUAL_GYRO
 
 // OMNIBUS F7 NEXT has two IMUs - MPU6000 onboard and ICM20608 (MPU6500) in the vibration dampened box
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PB12
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 #define MPU6500_CS_PIN          PA8
 #define MPU6500_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h
index 2d167eaeac..1d7f31c531 100644
--- a/src/main/target/PIKOBLX/target.h
+++ b/src/main/target/PIKOBLX/target.h
@@ -19,7 +19,7 @@
 
 #define TARGET_BOARD_IDENTIFIER "PIKO" // Furious FPV Piko BLX
 
-#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
+#define CONFIG_FASTLOOP_PREFERRED_ACC IMU_DEFAULT
 
 #define LED0                    PB9
 #define LED1                    PB5
@@ -32,14 +32,8 @@
 #define GYRO_INT_EXTI            PA15
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
-
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PB12
 #define MPU6000_SPI_BUS         BUS_SPI2
 
diff --git a/src/main/target/PIXRACER/target.c b/src/main/target/PIXRACER/target.c
index fcd15cde95..b881223337 100755
--- a/src/main/target/PIXRACER/target.c
+++ b/src/main/target/PIXRACER/target.c
@@ -21,19 +21,20 @@
 #include "drivers/io.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
+#include "drivers/sensor.h"
 #include "drivers/bus.h"
 
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_SPI_BUS,    MPU9250_CS_PIN,     MPU9250_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_icm20608,    DEVHW_MPU6500,      ICM20608_SPI_BUS,   ICM20608_CS_PIN,    ICM20608_EXTI_PIN,      1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_SPI_BUS,    MPU9250_CS_PIN,     MPU9250_EXTI_PIN,       0,  DEVFLAGS_NONE,  IMU_MPU9250_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm20608,    DEVHW_MPU6500,      ICM20608_SPI_BUS,   ICM20608_CS_PIN,    ICM20608_EXTI_PIN,      1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
-BUSDEV_REGISTER_SPI_TAG(busdev_ms5611,      DEVHW_MS5611,       MS5611_SPI_BUS,     MS5611_CS_PIN,      NONE,                   0,   DEVFLAGS_USE_RAW_REGISTERS);
+BUSDEV_REGISTER_SPI_TAG(busdev_ms5611,      DEVHW_MS5611,       MS5611_SPI_BUS,     MS5611_CS_PIN,      NONE,                   0,   DEVFLAGS_USE_RAW_REGISTERS,    0);
 
-BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS,        0x1E,               NONE,                   0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C_TAG(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS,        0x0D,               NONE,                   0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C_TAG(busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS,        0x0E,               NONE,                   0,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS,        0x1E,               NONE,                   0,  DEVFLAGS_NONE,    0);
+BUSDEV_REGISTER_I2C_TAG(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS,        0x0D,               NONE,                   0,  DEVFLAGS_NONE,    0);
+BUSDEV_REGISTER_I2C_TAG(busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS,        0x0E,               NONE,                   0,  DEVFLAGS_NONE,    0);
 
 // PixRacer has built-in HMC5983 compass on the same SPI bus as MPU9250
-BUSDEV_REGISTER_SPI_TAG(busdev_hmc5983_spi, DEVHW_HMC5883,      MPU9250_SPI_BUS,    PE15,               NONE,                   1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_hmc5983_spi, DEVHW_HMC5883,      MPU9250_SPI_BUS,    PE15,               NONE,                   1,  DEVFLAGS_NONE,    0);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH3, PB0,  TIM_USE_PPM,         0, 0 ),   // PPM shared uart6 pc7
diff --git a/src/main/target/PIXRACER/target.h b/src/main/target/PIXRACER/target.h
index a695ebcc85..5d6f71b1c8 100755
--- a/src/main/target/PIXRACER/target.h
+++ b/src/main/target/PIXRACER/target.h
@@ -39,15 +39,11 @@
 
 #define INVERTER_PIN_UART       PC13
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU9250
-#define USE_ACC_MPU9250
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG_FLIP
 
-#define GYRO_MPU6500_ALIGN      CW180_DEG_FLIP
-#define ACC_MPU6500_ALIGN       CW180_DEG_FLIP
-#define GYRO_MPU9250_ALIGN      CW180_DEG_FLIP
-#define ACC_MPU9250_ALIGN       CW180_DEG_FLIP
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG_FLIP
 #define MAG_MPU9250_ALIGN       CW90_DEG
 
 #define USE_DUAL_GYRO
@@ -62,9 +58,6 @@
 #define MPU9250_EXTI_PIN        PD15
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_GYRO
-
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
 #define USE_MAG_MPU9250
diff --git a/src/main/target/RADIX/target.h b/src/main/target/RADIX/target.h
index fbf2339a88..2f778992b5 100644
--- a/src/main/target/RADIX/target.h
+++ b/src/main/target/RADIX/target.h
@@ -31,13 +31,8 @@
 #define BMI160_CS_PIN           PB4
 #define GYRO_EXTI_PIN           PC13
 
-#define USE_GYRO
-#define USE_GYRO_BMI160
-#define GYRO_BMI160_ALIGN       CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_BMI160
-#define ACC_BMI160_ALIGN        CW0_DEG
+#define USE_IMU_BMI160
+#define IMU_BMI160_ALIGN        CW0_DEG
 
 // #define USE_MAG
 // #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h
index 53cc0371dd..738ed44638 100644
--- a/src/main/target/RCEXPLORERF3/target.h
+++ b/src/main/target/RCEXPLORERF3/target.h
@@ -29,16 +29,11 @@
 #define GYRO_INT_EXTI            PA15
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PB12
 #define MPU6000_SPI_BUS         BUS_SPI2
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C2
diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c
index 3f7431af59..eb02a3eb59 100644
--- a/src/main/target/REVO/target.c
+++ b/src/main/target/REVO/target.c
@@ -23,13 +23,13 @@
 #include "drivers/timer.h"
 #include "drivers/timer_def.h"
 #include "drivers/pwm_mapping.h"
+#include "drivers/sensor.h"
 
-/* GYRO */
-BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883_int, DEVHW_HMC5883,      MAG_I2C_BUS_INT,    0x1E,               NONE,                   0,  DEVFLAGS_NONE);
-
-BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS_EXT,    0x1E,               NONE,                   1,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C_TAG(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS_EXT,    0x0D,               NONE,                   1,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C_TAG(busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS_EXT,    0x0E,               NONE,                   1,  DEVFLAGS_NONE);
+/* COMPAS */
+BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883_int, DEVHW_HMC5883,      MAG_I2C_BUS_INT,    0x1E,               NONE,                   0,  DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS_EXT,    0x1E,               NONE,                   1,  DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C_TAG(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS_EXT,    0x0D,               NONE,                   1,  DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C_TAG(busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS_EXT,    0x0E,               NONE,                   1,  DEVFLAGS_NONE,  0);
 
 
 /* TIMERS */
diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h
index 3c1c952e54..be2c511c19 100644
--- a/src/main/target/REVO/target.h
+++ b/src/main/target/REVO/target.h
@@ -20,9 +20,6 @@
 #define TARGET_BOARD_IDENTIFIER "REVO"
 
 #define USBD_PRODUCT_STRING     "Revolution"
-#ifdef OPBL
-#define USBD_SERIALNUMBER_STRING "0x8020000"
-#endif
 
 // Use target-specific MAG hardware descriptors (don't use common_hardware.h)
 #define USE_TARGET_MAG_HARDWARE_DESCRIPTORS
@@ -41,17 +38,11 @@
 #define GYRO_INT_EXTI           PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
 #define USE_MAG
 #define USE_DUAL_MAG
 #define MAG_I2C_BUS_EXT         BUS_I2C2
diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h
index 1e78f6df93..fe467f8296 100644
--- a/src/main/target/RMDO/target.h
+++ b/src/main/target/RMDO/target.h
@@ -28,15 +28,10 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_ACC
-
-#define USE_GYRO_MPU6050
-#define USE_ACC_MPU6050
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 #define MPU6050_I2C_BUS         BUS_I2C1
 
-#define GYRO_MPU6050_ALIGN      CW270_DEG
-#define ACC_MPU6050_ALIGN       CW270_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index 6b028cb0d6..26baef81c2 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -31,14 +31,8 @@
 #define USE_MPU_DATA_READY_SIGNAL
 
 // MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component.
-#define USE_GYRO
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW270_DEG
-
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 #define MPU6050_I2C_BUS         BUS_I2C2
 
 #define USE_BARO
diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h
index adff656ca7..f716369b74 100644
--- a/src/main/target/SPARKY2/target.h
+++ b/src/main/target/SPARKY2/target.h
@@ -33,13 +33,8 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 
 #define MPU9250_SPI_BUS         BUS_SPI1
 #define MPU9250_CS_PIN          PC4
diff --git a/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL1.mk b/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL1.mk
new file mode 100644
index 0000000000..123317b4ed
--- /dev/null
+++ b/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL1.mk
@@ -0,0 +1 @@
+#SPEEDYBEEF4_SFTSRL1
\ No newline at end of file
diff --git a/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL2.mk b/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL2.mk
new file mode 100644
index 0000000000..6cc372b363
--- /dev/null
+++ b/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL2.mk
@@ -0,0 +1 @@
+#SPEEDYBEEF4_SFTSRL2
\ No newline at end of file
diff --git a/src/main/target/SPEEDYBEEF4/target.c b/src/main/target/SPEEDYBEEF4/target.c
index 45a98da28b..8fa896dae5 100644
--- a/src/main/target/SPEEDYBEEF4/target.c
+++ b/src/main/target/SPEEDYBEEF4/target.c
@@ -26,8 +26,8 @@
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM5, CH4, PA3,  TIM_USE_PPM, 0, 0), // PPM
 
-    DEF_TIM(TIM8, CH1, PC6,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1  UP(1,2)
-    DEF_TIM(TIM8, CH2, PC7,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2  UP(2,1)
+    DEF_TIM(TIM3, CH1, PC6,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1  UP(1,2)
+    DEF_TIM(TIM3, CH2, PC7,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2  UP(2,1)
     DEF_TIM(TIM3, CH3, PC8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3  UP(2,1)
     DEF_TIM(TIM3, CH4, PC9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4  UP(2,1)
 
diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h
index 0e7f8e2c53..1365b7aec5 100644
--- a/src/main/target/SPEEDYBEEF4/target.h
+++ b/src/main/target/SPEEDYBEEF4/target.h
@@ -17,60 +17,68 @@
 
 #pragma once
 
-#define TARGET_BOARD_IDENTIFIER "SBF4"
-#define USBD_PRODUCT_STRING  "SpeedyBeeF4"
+#define TARGET_BOARD_IDENTIFIER "SBF4"		
+#define USBD_PRODUCT_STRING  "SpeedyBeeF4"	
 
-#define LED0                    PB9
+/*** Indicators ***/
+#define LED0                    PB9			
+#define BEEPER                  PC13		
+#define BEEPER_INVERTED						
 
-#define BEEPER                  PC13
-#define BEEPER_INVERTED
+/*** IMU sensors ***/
+#define USE_EXTI							
 
-#define USE_I2C
-#define USE_I2C_DEVICE_1
-#define I2C1_SCL                PB6        // SCL pad
-#define I2C1_SDA                PB7        // SDA pad
-#define I2C_DEVICE              (I2CDEV_1)
+#define GYRO_INT_EXTI            PC4		
+#define USE_MPU_DATA_READY_SIGNAL			
 
-#define UG2864_I2C_BUS BUS_I2C1
-
-// MPU6000 interrupts
-#define USE_EXTI
-#define GYRO_INT_EXTI            PC4
-#define USE_MPU_DATA_READY_SIGNAL
+#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO)
 
+/*** MPU6000 ***/
+#define USE_IMU_MPU6000
 #define MPU6000_CS_PIN          PB11
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW0_DEG
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW0_DEG
+#define IMU_MPU6000_ALIGN       CW0_DEG
 
-#define USE_MAG
-#define MAG_I2C_BUS             BUS_I2C1
-#define MAG_HMC5883_ALIGN       CW90_DEG
-#define USE_MAG_HMC5883
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_MAG3110
-#define USE_MAG_LIS3MDL
+/*** SPI/I2C bus ***/
+#define USE_SPI								
+#define USE_SPI_DEVICE_1					
+#define SPI1_SCK_PIN            PA5			
+#define SPI1_MISO_PIN           PA6			
+#define SPI1_MOSI_PIN           PA7			
 
-#define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define USE_SPI_DEVICE_2					
+#define SPI2_NSS_PIN            PB12		
+#define SPI2_SCK_PIN            PB13		
+#define SPI2_MISO_PIN           PB14		
+#define SPI2_MOSI_PIN           PB15		
 
-#define USE_BARO
+#define USE_SPI_DEVICE_3					
+#define SPI3_SCK_PIN            PB3			
+#define SPI3_MISO_PIN           PB4			
+#define SPI3_MOSI_PIN           PB5			
 
-#define BARO_I2C_BUS          BUS_I2C1
-#define USE_BARO_BMP085
-#define USE_BARO_BMP280
-#define USE_BARO_MS5611
+#define I2C_DEVICE              (I2CDEV_1)	
+#define UG2864_I2C_BUS BUS_I2C1				
 
-#define PITOT_I2C_BUS           BUS_I2C1
+#define USE_I2C								
+#define USE_I2C_DEVICE_1					
+#define I2C1_SCL                PB6        	
+#define I2C1_SDA                PB7        	
 
-#define USE_RANGEFINDER
-#define RANGEFINDER_I2C_BUS     BUS_I2C1
-#define USE_RANGEFINDER_HCSR04_I2C
+/*** Onboard flash ***/
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define M25P16_CS_PIN           PC0				
+#define M25P16_SPI_BUS          BUS_SPI3		
+#define USE_FLASHFS								
+#define USE_FLASH_M25P16	
 
+/*** OSD ***/
+#define USE_OSD								
+#define USE_MAX7456							
+#define MAX7456_SPI_BUS         BUS_SPI2	
+#define MAX7456_CS_PIN          PB10		
+
+/*** Serial ports ***/
 #define USE_VCP
 #define VBUS_SENSING_PIN        PB12
 #define VBUS_SENSING_ENABLED
@@ -95,40 +103,44 @@
 #define UART5_RX_PIN            PD2
 #define UART5_TX_PIN            PC12
 
+#if defined(SPEEDYBEEF4_SFTSRL1)
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_RX_PIN     PA15 // S5
+#define SOFTSERIAL_1_TX_PIN     PB8  // S7
+
+#define SERIAL_PORT_COUNT       7
+
+#elif defined(SPEEDYBEEF4_SFTSRL2)
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_RX_PIN     PA15 // S5
+#define SOFTSERIAL_1_TX_PIN     PB8  // S7
+#define USE_SOFTSERIAL2
+#define SOFTSERIAL_2_RX_PIN     PA8 // S6   
+#define SOFTSERIAL_2_TX_PIN     PA4 // DAC
+
+#define SERIAL_PORT_COUNT       8
+
+#else
 #define SERIAL_PORT_COUNT       6
+#endif
 
-#define USE_SPI
+/*** BARO & MAG ***/
+#define USE_BARO							
+#define BARO_I2C_BUS          	BUS_I2C1	
+#define USE_BARO_BMP085						
+#define USE_BARO_BMP280						
+#define USE_BARO_MS5611						
 
-#define USE_SPI_DEVICE_1
-#define SPI1_SCK_PIN            PA5
-#define SPI1_MISO_PIN           PA6
-#define SPI1_MOSI_PIN           PA7
-
-
-#define USE_SPI_DEVICE_2
-#define SPI2_NSS_PIN            PB12
-#define SPI2_SCK_PIN            PB13
-#define SPI2_MISO_PIN           PB14
-#define SPI2_MOSI_PIN           PB15
-
-#define USE_SPI_DEVICE_3
-#define SPI3_NSS_PIN            PC0
-#define SPI3_SCK_PIN            PB3
-#define SPI3_MISO_PIN           PB4
-#define SPI3_MOSI_PIN           PB5
-
-
-#define USE_OSD
-#define USE_MAX7456
-#define MAX7456_SPI_BUS         BUS_SPI2
-#define MAX7456_CS_PIN          PB10
-
-#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
-#define M25P16_CS_PIN           SPI3_NSS_PIN
-#define M25P16_SPI_BUS          BUS_SPI3
-#define USE_FLASHFS
-#define USE_FLASH_M25P16
+#define USE_MAG								
+#define MAG_I2C_BUS             BUS_I2C1	
+#define USE_MAG_HMC5883						
+#define USE_MAG_QMC5883						
+#define USE_MAG_IST8310						
+#define USE_MAG_MAG3110						
+#define USE_MAG_LIS3MDL						
+#define MAG_HMC5883_ALIGN       CW90_DEG	
 
+/*** ADC ***/
 #define USE_ADC
 #define ADC_CHANNEL_1_PIN               PC1
 #define ADC_CHANNEL_2_PIN               PC2
@@ -138,24 +150,22 @@
 #define VBAT_ADC_CHANNEL                ADC_CHN_2
 #define RSSI_ADC_CHANNEL                ADC_CHN_3
 
-#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO)
+/*** LED STRIP ***/
 
-#define DEFAULT_RX_TYPE         RX_TYPE_PPM
-#define DISABLE_RX_PWM_FEATURE
+/*** Default settings ***/
 #define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD)
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define CURRENT_METER_SCALE     166
 
-#define USE_SPEKTRUM_BIND
-#define BIND_PIN                PC11 // USART3 RX
-
+/*** Timer/PWM output ***/
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
-
-// Number of available PWM outputs
 #define MAX_PWM_OUTPUT_PORTS    7
-#define TARGET_MOTOR_COUNT      7
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
+/*** Used pins ***/
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff
 #define TARGET_IO_PORTC         0xffff
 #define TARGET_IO_PORTD         (BIT(2))
-
-#define CURRENT_METER_SCALE   302
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index 4717440c16..05727f5d42 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -30,14 +30,8 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW270_DEG
-
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 #define MPU6050_I2C_BUS         BUS_I2C1
 
 #define USE_BARO
diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h
index 7a4f57490a..c2b139a044 100755
--- a/src/main/target/SPRACINGF3EVO/target.h
+++ b/src/main/target/SPRACINGF3EVO/target.h
@@ -35,17 +35,10 @@
 #define MPU9250_CS_PIN          PB9
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h
index 758bd30577..6a7ec647f1 100644
--- a/src/main/target/SPRACINGF3MINI/target.h
+++ b/src/main/target/SPRACINGF3MINI/target.h
@@ -33,17 +33,10 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG
 
 #define MPU6500_I2C_BUS         BUS_I2C1
 #define MPU9250_I2C_BUS         BUS_I2C1
diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h
index e8ff5509e1..9bec2fda3c 100755
--- a/src/main/target/SPRACINGF3NEO/target.h
+++ b/src/main/target/SPRACINGF3NEO/target.h
@@ -30,18 +30,11 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define USE_GYRO_MPU9250
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define USE_ACC_MPU9250
-
-#define ACC_MPU6500_ALIGN       CW0_DEG
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-#define ACC_MPU9250_ALIGN       CW0_DEG
-#define GYRO_MPU9250_ALIGN      CW0_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW0_DEG
 
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h
index 04f92232f4..c1da04f60b 100755
--- a/src/main/target/SPRACINGF4EVO/target.h
+++ b/src/main/target/SPRACINGF4EVO/target.h
@@ -37,19 +37,11 @@
 #define USE_MAG_DATA_READY_SIGNAL
 #define ENSURE_MAG_DATA_READY_IS_HIGH
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define USE_GYRO_MPU9250
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define USE_ACC_MPU9250
-
-#define ACC_MPU6500_ALIGN       CW0_DEG
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-
-#define ACC_MPU9250_ALIGN       CW0_DEG
-#define GYRO_MPU9250_ALIGN      CW0_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW0_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/SPRACINGF7DUAL/target.c b/src/main/target/SPRACINGF7DUAL/target.c
index d9c1c21b9a..8fb1c781f5 100644
--- a/src/main/target/SPRACINGF7DUAL/target.c
+++ b/src/main/target/SPRACINGF7DUAL/target.c
@@ -27,10 +27,11 @@
 #include "drivers/timer_def.h"
 #include "drivers/dma.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 // Register both MPU6500
-BUSDEV_REGISTER_SPI(busdev_mpu6500_1,     DEVHW_MPU6500,      MPU6500_1_SPI_BUS,    MPU6500_1_CS_PIN,     GYRO_1_EXTI_PIN,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI(busdev_mpu6500_2,     DEVHW_MPU6500,      MPU6500_2_SPI_BUS,    MPU6500_2_CS_PIN,     GYRO_2_EXTI_PIN,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500_1,     DEVHW_MPU6500,      MPU6500_1_SPI_BUS,    MPU6500_1_CS_PIN,     GYRO_1_EXTI_PIN,  0,  DEVFLAGS_NONE,  IMU_1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500_2,     DEVHW_MPU6500,      MPU6500_2_SPI_BUS,    MPU6500_2_CS_PIN,     GYRO_2_EXTI_PIN,  0,  DEVFLAGS_NONE,  IMU_2_ALIGN);
 
 const timerHardware_t timerHardware[] = {
 
diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h
index 3f65888364..aead3cbe59 100644
--- a/src/main/target/SPRACINGF7DUAL/target.h
+++ b/src/main/target/SPRACINGF7DUAL/target.h
@@ -39,41 +39,24 @@
 #define USE_EXTI
 #define GYRO_1_EXTI_PIN         PC13
 #define GYRO_2_EXTI_PIN         PC14
-#define GYRO_INT_EXTI           GYRO_1_EXTI_PIN
 
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
 #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
-#define USE_GYRO
-#define USE_GYRO_MPU6500
 
-#define USE_ACC
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_1_ALIGN                 IMU_MPU6500_1_ALIGN
+#define IMU_2_ALIGN                 IMU_MPU6500_2_ALIGN
 
 #if (SPRACINGF7DUAL_REV >= 2)
-#define ACC_MPU6500_1_ALIGN           CW0_DEG
-#define GYRO_MPU6500_1_ALIGN          CW0_DEG
-
-#define ACC_MPU6500_2_ALIGN         CW270_DEG
-#define GYRO_MPU6500_2_ALIGN        CW270_DEG
+#define IMU_MPU6500_1_ALIGN         CW0_DEG
+#define IMU_MPU6500_2_ALIGN         CW270_DEG
 #else
-#define ACC_MPU6500_1_ALIGN           CW180_DEG
-#define GYRO_MPU6500_1_ALIGN          CW180_DEG
-
-#define ACC_MPU6500_2_ALIGN         CW270_DEG
-#define GYRO_MPU6500_2_ALIGN        CW270_DEG
+#define IMU_MPU6500_1_ALIGN         CW180_DEG
+#define IMU_MPU6500_2_ALIGN         CW270_DEG
 #endif
 
-
-#define GYRO_1_ALIGN                GYRO_MPU6500_1_ALIGN
-#define GYRO_2_ALIGN                GYRO_MPU6500_2_ALIGN
-#define ACC_1_ALIGN                 ACC_MPU6500_1_ALIGN
-#define ACC_2_ALIGN                 ACC_MPU6500_2_ALIGN
-
-#define ACC_MPU6500_ALIGN       ACC_1_ALIGN
-#define GYRO_MPU6500_ALIGN      GYRO_1_ALIGN
-
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
 #define USE_BARO_BMP280
diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h
index 0e95781fd6..dc423616f2 100644
--- a/src/main/target/YUPIF4/target.h
+++ b/src/main/target/YUPIF4/target.h
@@ -53,17 +53,8 @@
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
-
-#define USE_ACC
-#define USE_ACC_SPI_MPU6500
-#define ACC_MPU6500_ALIGN       CW90_DEG
-
-#define USE_GYRO
-#define USE_GYRO_SPI_MPU6500
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 
 #define USE_I2C
 #define USE_I2C_DEVICE_2
diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h
index 84a9c2dd08..b7799bfb81 100644
--- a/src/main/target/YUPIF7/target.h
+++ b/src/main/target/YUPIF7/target.h
@@ -40,16 +40,8 @@
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
-
-#define USE_ACC
-#define USE_ACC_SPI_MPU6500
-#define ACC_MPU6500_ALIGN       CW90_DEG
-
-#define USE_GYRO
-#define USE_GYRO_SPI_MPU6500
-#define GYRO_MPU6500_ALIGN      CW90_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 5c8803245f..52eb6eb819 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -46,6 +46,7 @@
 
 #if defined(STM32F4) || defined(STM32F7)
 #define USE_USB_MSC
+#define USE_SERVO_SBUS
 #endif
 
 #define USE_ADC_AVERAGING
@@ -58,8 +59,6 @@
 #define USE_TELEMETRY_LTM
 #define USE_TELEMETRY_FRSKY
 
-#define USE_MR_BRAKING_MODE
-
 #if defined(STM_FAST_TARGET)
 #define SCHEDULER_DELAY_LIMIT           10
 #else
@@ -67,6 +66,11 @@
 #endif
 
 #if (FLASH_SIZE > 256)
+#define USE_MR_BRAKING_MODE
+#define USE_PITOT
+#define USE_PITOT_ADC
+#define USE_PITOT_VIRTUAL
+
 #define USE_DYNAMIC_FILTERS
 #define USE_EXTENDED_CMS_MENUS
 #define USE_UAV_INTERCONNECT
@@ -149,9 +153,6 @@
 #define USE_SERIAL_PASSTHROUGH
 #define NAV_MAX_WAYPOINTS       60
 #define USE_RCDEVICE
-#define USE_PITOT
-#define USE_PITOT_ADC
-#define USE_PITOT_VIRTUAL
 
 //Enable VTX control
 #define USE_VTX_CONTROL
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index b4fadfca59..1e35d2baef 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -18,6 +18,7 @@
 #include <platform.h>
 #include "drivers/io.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 #if !defined(USE_TARGET_HARDWARE_DESCRIPTORS)
 
@@ -31,47 +32,52 @@
     #define MPU_ADDRESS 0x68
     #endif
 
-    #if defined(USE_GYRO_L3GD20)
-        BUSDEV_REGISTER_SPI(busdev_l3gd20,      DEVHW_L3GD20,       L3GD20_SPI_BUS,     L3GD20_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    #if defined(USE_IMU_L3GD20)
+        #if defined(GYRO_L3GD20_ALIGN)
+            #define GYRO_0_ALIGN    GYRO_L3GD20_ALIGN
+        #else
+            #define GYRO_0_ALIGN    ALIGN_DEFAULT
+        #endif
+        BUSDEV_REGISTER_SPI(busdev_l3gd20,      DEVHW_L3GD20,       L3GD20_SPI_BUS,     L3GD20_CS_PIN,      NONE,           DEVFLAGS_NONE,  IMU_L3GD20_ALIGN);
     #endif
 
-    #if defined(USE_ACC_LSM303DLHC)
-        BUSDEV_REGISTER_I2C(busdev_lsm303,      DEVHW_LSM303DLHC,   LSM303DLHC_I2C_BUS, 0x19,               NONE,           DEVFLAGS_NONE);
+    #if defined(USE_IMU_LSM303DLHC)
+        BUSDEV_REGISTER_I2C(busdev_lsm303,      DEVHW_LSM303DLHC,   LSM303DLHC_I2C_BUS, 0x19,               NONE,           DEVFLAGS_NONE,  IMU_LSM303DLHC_ALIGN);
     #endif
 
-    #if defined(USE_GYRO_MPU6000)
-        BUSDEV_REGISTER_SPI(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE);
+    #if defined(USE_IMU_MPU6000)
+        BUSDEV_REGISTER_SPI(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
     #endif
 
-    #if defined(USE_GYRO_MPU6050)
-        BUSDEV_REGISTER_I2C(busdev_mpu6050,     DEVHW_MPU6050,      MPU6050_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE);
+    #if defined(USE_IMU_MPU6050)
+        BUSDEV_REGISTER_I2C(busdev_mpu6050,     DEVHW_MPU6050,      MPU6050_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6050_ALIGN);
     #endif
 
-    #if defined(USE_GYRO_MPU6500)
+    #if defined(USE_IMU_MPU6500)
         #if defined(MPU6500_SPI_BUS)
-        BUSDEV_REGISTER_SPI(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_SPI(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
         #elif defined(MPU6500_I2C_BUS)
-        BUSDEV_REGISTER_I2C(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
         #endif
     #endif
 
-    #if defined(USE_GYRO_MPU9250)
+    #if defined(USE_IMU_MPU9250)
         #if defined(MPU9250_SPI_BUS)
-        BUSDEV_REGISTER_SPI(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_SPI_BUS,    MPU9250_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_SPI(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_SPI_BUS,    MPU9250_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU9250_ALIGN);
         #elif defined(MPU9250_I2C_BUS)
-        BUSDEV_REGISTER_I2C(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU9250_ALIGN);
         #endif
     #endif
 
-    #if defined(USE_GYRO_ICM20689)
-        BUSDEV_REGISTER_SPI(busdev_icm20689,    DEVHW_ICM20689,     ICM20689_SPI_BUS,   ICM20689_CS_PIN,    GYRO_INT_EXTI,  DEVFLAGS_NONE);
+    #if defined(USE_IMU_ICM20689)
+        BUSDEV_REGISTER_SPI(busdev_icm20689,    DEVHW_ICM20689,     ICM20689_SPI_BUS,   ICM20689_CS_PIN,    GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_ICM20689_ALIGN);
     #endif
 
-    #if defined(USE_GYRO_BMI160)
+    #if defined(USE_IMU_BMI160)
         #if defined(BMI160_SPI_BUS)
-        BUSDEV_REGISTER_SPI(busdev_bmi160,      DEVHW_BMI160,       BMI160_SPI_BUS,     BMI160_CS_PIN,      GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_SPI(busdev_bmi160,      DEVHW_BMI160,       BMI160_SPI_BUS,     BMI160_CS_PIN,      GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI160_ALIGN);
         #elif defined(BMI160_I2C_BUS)
-        BUSDEV_REGISTER_I2C(busdev_bmi160,      DEVHW_BMI160,       BMI160_I2C_BUS,     0x68,               GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_bmi160,      DEVHW_BMI160,       BMI160_I2C_BUS,     0x68,               GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI160_ALIGN);
         #endif
     #endif
 #endif
@@ -83,45 +89,45 @@
     #if !defined(BMP085_I2C_BUS)
         #define BMP085_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_bmp085,      DEVHW_BMP085,       BMP085_I2C_BUS,     0x77,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_bmp085,      DEVHW_BMP085,       BMP085_I2C_BUS,     0x77,               NONE,           DEVFLAGS_NONE,      0);
 #endif
 
 #if defined(USE_BARO_BMP280)
     #if defined(BMP280_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_bmp280,      DEVHW_BMP280,       BMP280_SPI_BUS,     BMP280_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_bmp280,      DEVHW_BMP280,       BMP280_SPI_BUS,     BMP280_CS_PIN,      NONE,           DEVFLAGS_NONE,      0);
     #elif defined(BMP280_I2C_BUS) || defined(BARO_I2C_BUS)
     #if !defined(BMP280_I2C_BUS)
         #define BMP280_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_bmp280,      DEVHW_BMP280,       BMP280_I2C_BUS,     0x76,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_bmp280,      DEVHW_BMP280,       BMP280_I2C_BUS,     0x76,               NONE,           DEVFLAGS_NONE,      0);
     #endif
 #endif
 
 #if defined(USE_BARO_BMP388)
     #if defined(BMP388_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_bmp388,      DEVHW_BMP388,       BMP388_SPI_BUS,     BMP388_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_bmp388,      DEVHW_BMP388,       BMP388_SPI_BUS,     BMP388_CS_PIN,      NONE,           DEVFLAGS_NONE,      0);
     #elif defined(BMP388_I2C_BUS) || defined(BARO_I2C_BUS)
     #if !defined(BMP388_I2C_BUS)
         #define BMP388_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_bmp388,      DEVHW_BMP388,       BMP388_I2C_BUS,     0x76,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_bmp388,      DEVHW_BMP388,       BMP388_I2C_BUS,     0x76,               NONE,           DEVFLAGS_NONE,      0);
     #endif
 #endif
 
 #if defined(USE_BARO_SPL06)
     #if defined(SPL06_SPI_BUS)
-      BUSDEV_REGISTER_SPI(busdev_spl06,     DEVHW_SPL06,        SPL06_SPI_BUS,      SPL06_CS_PIN,       NONE,           DEVFLAGS_NONE);
+      BUSDEV_REGISTER_SPI(busdev_spl06,     DEVHW_SPL06,        SPL06_SPI_BUS,      SPL06_CS_PIN,       NONE,           DEVFLAGS_NONE,      0);
     #elif defined(SPL06_I2C_BUS) || defined(BARO_I2C_BUS)
       #if !defined(SPL06_I2C_BUS)
         #define SPL06_I2C_BUS BARO_I2C_BUS
       #endif
-      BUSDEV_REGISTER_I2C(busdev_spl06,     DEVHW_SPL06,        SPL06_I2C_BUS,      0x76,               NONE,           DEVFLAGS_NONE);
+      BUSDEV_REGISTER_I2C(busdev_spl06,     DEVHW_SPL06,        SPL06_I2C_BUS,      0x76,               NONE,           DEVFLAGS_NONE,      0);
     #endif
 #endif
 
 #if defined(USE_BARO_LPS25H)
     #if defined(LPS25H_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_lps25h,      DEVHW_LPS25H,       LPS25H_SPI_BUS,     LPS25H_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_lps25h,      DEVHW_LPS25H,       LPS25H_SPI_BUS,     LPS25H_CS_PIN,      NONE,           DEVFLAGS_NONE,      0);
     #endif
 #endif
 
@@ -129,17 +135,17 @@
     #if !defined(MS5607_I2C_BUS)
         #define MS5607_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ms5607,      DEVHW_MS5607,       MS5607_I2C_BUS,     0x77,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_I2C(busdev_ms5607,      DEVHW_MS5607,       MS5607_I2C_BUS,     0x77,               NONE,           DEVFLAGS_USE_RAW_REGISTERS, 0);
 #endif
 
 #if defined(USE_BARO_MS5611)
     #if defined(MS5611_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_ms5611,      DEVHW_MS5611,       MS5611_SPI_BUS,     MS5611_CS_PIN,      NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_SPI(busdev_ms5611,      DEVHW_MS5611,       MS5611_SPI_BUS,     MS5611_CS_PIN,      NONE,           DEVFLAGS_USE_RAW_REGISTERS, 0);
     #elif defined(MS5611_I2C_BUS) || defined(BARO_I2C_BUS)
     #if !defined(MS5611_I2C_BUS)
         #define MS5611_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ms5611,      DEVHW_MS5611,       MS5611_I2C_BUS,     0x77,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_I2C(busdev_ms5611,      DEVHW_MS5611,       MS5611_I2C_BUS,     0x77,               NONE,           DEVFLAGS_USE_RAW_REGISTERS, 0);
     #endif
 #endif
 
@@ -149,35 +155,35 @@
     #if !defined(HMC5883_I2C_BUS)
         #define HMC5883_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_hmc5883,     DEVHW_HMC5883,      HMC5883_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_hmc5883,     DEVHW_HMC5883,      HMC5883_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_MAG_QMC5883)
     #if !defined(QMC5883_I2C_BUS)
         #define QMC5883_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_qmc5883,     DEVHW_QMC5883,      QMC5883_I2C_BUS,    0x0D,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_qmc5883,     DEVHW_QMC5883,      QMC5883_I2C_BUS,    0x0D,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_MAG_AK8963)
     #if defined(AK8963_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_ak8963,      DEVHW_AK8963,       AK8963_SPI_BUS,     AK8963_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_ak8963,      DEVHW_AK8963,       AK8963_SPI_BUS,     AK8963_CS_PIN,      NONE,           DEVFLAGS_NONE,  0);
     #elif defined(AK8963_I2C_BUS) || defined(MAG_I2C_BUS)
     #if !defined(AK8963_I2C_BUS)
         #define AK8963_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ak8963,      DEVHW_AK8963,       AK8963_I2C_BUS,     0x0C,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ak8963,      DEVHW_AK8963,       AK8963_I2C_BUS,     0x0C,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
 #if defined(USE_MAG_AK8975)
     #if defined(AK8975_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_ak8975,      DEVHW_AK8975,       AK8975_SPI_BUS,     AK8975_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_ak8975,      DEVHW_AK8975,       AK8975_SPI_BUS,     AK8975_CS_PIN,      NONE,           DEVFLAGS_NONE,  0);
     #elif defined(AK8975_I2C_BUS) || defined(MAG_I2C_BUS)
     #if !defined(AK8975_I2C_BUS)
         #define AK8975_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ak8975,      DEVHW_AK8975,       AK8975_I2C_BUS,     0x0C,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ak8975,      DEVHW_AK8975,       AK8975_I2C_BUS,     0x0C,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
@@ -185,14 +191,14 @@
     #if !defined(MAG3110_I2C_BUS)
         #define MAG3110_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_mag3110,     DEVHW_MAG3110,      MAG3110_I2C_BUS,    0x0E,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_mag3110,     DEVHW_MAG3110,      MAG3110_I2C_BUS,    0x0E,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_MAG_LIS3MDL)
     #if !defined(LIS3MDL_I2C_BUS)
         #define LIS3MDL_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_lis3mdl,     DEVHW_LIS3MDL,      LIS3MDL_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_lis3mdl,     DEVHW_LIS3MDL,      LIS3MDL_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE,  0);
 //  ST LIS3MDL address can be changed by connecting it's SDO/SA1 pin to either supply or ground.
 //  BUSDEV_REGISTER_I2C(busdev_lis3mdl,     DEVHW_LIS3MDL,      LIS3MDL_I2C_BUS,    0x1C,               NONE,           DEVFLAGS_NONE);
 #endif
@@ -201,15 +207,15 @@
     #if !defined(IST8310_I2C_BUS)
         #define IST8310_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ist8310_0,   DEVHW_IST8310_0,    IST8310_I2C_BUS,    0x0C,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_ist8310_1,   DEVHW_IST8310_1,    IST8310_I2C_BUS,    0x0E,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ist8310_0,   DEVHW_IST8310_0,    IST8310_I2C_BUS,    0x0C,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_ist8310_1,   DEVHW_IST8310_1,    IST8310_I2C_BUS,    0x0E,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_MAG_IST8308)
     #if !defined(IST8308_I2C_BUS)
         #define IST8308_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ist8308,     DEVHW_IST8308,      IST8308_I2C_BUS,    0x0C,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ist8308,     DEVHW_IST8308,      IST8308_I2C_BUS,    0x0C,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 #endif
 
@@ -223,7 +229,7 @@
 #endif
 
 #if defined(USE_1WIRE_DS2482) && defined(DS2482_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_ds2482,      DEVHW_DS2482,       DS2482_I2C_BUS,     0x18,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_I2C(busdev_ds2482,      DEVHW_DS2482,       DS2482_I2C_BUS,     0x18,               NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);
 #endif
 
 #endif
@@ -236,14 +242,14 @@
 #endif
 
 #if defined(USE_TEMPERATURE_LM75) && defined(LM75_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_lm75_0,      DEVHW_LM75_0,         LM75_I2C_BUS,     0x48,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_1,      DEVHW_LM75_1,         LM75_I2C_BUS,     0x49,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_2,      DEVHW_LM75_2,         LM75_I2C_BUS,     0x4A,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_3,      DEVHW_LM75_3,         LM75_I2C_BUS,     0x4B,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_4,      DEVHW_LM75_4,         LM75_I2C_BUS,     0x4C,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_5,      DEVHW_LM75_5,         LM75_I2C_BUS,     0x4D,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_6,      DEVHW_LM75_6,         LM75_I2C_BUS,     0x4E,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_7,      DEVHW_LM75_7,         LM75_I2C_BUS,     0x4F,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_lm75_0,      DEVHW_LM75_0,         LM75_I2C_BUS,     0x48,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_1,      DEVHW_LM75_1,         LM75_I2C_BUS,     0x49,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_2,      DEVHW_LM75_2,         LM75_I2C_BUS,     0x4A,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_3,      DEVHW_LM75_3,         LM75_I2C_BUS,     0x4B,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_4,      DEVHW_LM75_4,         LM75_I2C_BUS,     0x4C,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_5,      DEVHW_LM75_5,         LM75_I2C_BUS,     0x4D,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_6,      DEVHW_LM75_6,         LM75_I2C_BUS,     0x4E,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_7,      DEVHW_LM75_7,         LM75_I2C_BUS,     0x4F,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 
@@ -254,7 +260,7 @@
         #define SRF10_I2C_BUS RANGEFINDER_I2C_BUS
     #endif
     #if defined(SRF10_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_srf10,       DEVHW_SRF10,        SRF10_I2C_BUS,      0x70,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_srf10,       DEVHW_SRF10,        SRF10_I2C_BUS,      0x70,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
@@ -263,7 +269,7 @@
         #define HCSR04_I2C_BUS RANGEFINDER_I2C_BUS
     #endif
     #if defined(HCSR04_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_hcsr04,      DEVHW_HCSR04_I2C,   HCSR04_I2C_BUS,     0x14,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_hcsr04,      DEVHW_HCSR04_I2C,   HCSR04_I2C_BUS,     0x14,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
@@ -273,7 +279,7 @@
     #endif
 
     #if defined(VL53L0X_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_vl53l0x,     DEVHW_VL53L0X,      VL53L0X_I2C_BUS,    0x29,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_vl53l0x,     DEVHW_VL53L0X,      VL53L0X_I2C_BUS,    0x29,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
@@ -285,22 +291,22 @@
 #endif
 
 #if defined(USE_PITOT_MS4525) && defined(MS4525_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_ms5425,      DEVHW_MS4525,       MS4525_I2C_BUS,     0x28,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);    // Requires 0xFF to passthrough
+    BUSDEV_REGISTER_I2C(busdev_ms5425,      DEVHW_MS4525,       MS4525_I2C_BUS,     0x28,               NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);    // Requires 0xFF to passthrough
 #endif
 
 
 /** OTHER HARDWARE **/
 
 #if defined(USE_MAX7456)
-    BUSDEV_REGISTER_SPI(busdev_max7456,     DEVHW_MAX7456,      MAX7456_SPI_BUS,    MAX7456_CS_PIN,     NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_SPI(busdev_max7456,     DEVHW_MAX7456,      MAX7456_SPI_BUS,    MAX7456_CS_PIN,     NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);
 #endif
 
 #if defined(USE_FLASH_M25P16)
-    BUSDEV_REGISTER_SPI(busdev_m25p16,      DEVHW_M25P16,       M25P16_SPI_BUS,     M25P16_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_m25p16,      DEVHW_M25P16,       M25P16_SPI_BUS,     M25P16_CS_PIN,      NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_SDCARD) && defined(USE_SDCARD_SPI)
-    BUSDEV_REGISTER_SPI(busdev_sdcard_spi,  DEVHW_SDCARD,       SDCARD_SPI_BUS,     SDCARD_CS_PIN,      NONE,           DEVFLAGS_USE_MANUAL_DEVICE_SELECT | DEVFLAGS_SPI_MODE_0);
+    BUSDEV_REGISTER_SPI(busdev_sdcard_spi,  DEVHW_SDCARD,       SDCARD_SPI_BUS,     SDCARD_CS_PIN,      NONE,           DEVFLAGS_USE_MANUAL_DEVICE_SELECT | DEVFLAGS_SPI_MODE_0,  0);
 #endif
 
 /*
@@ -314,7 +320,7 @@
     #if !defined(UG2864_I2C_BUS)
         #define UG2864_I2C_BUS BUS_I2C1
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ug2864,      DEVHW_UG2864,       UG2864_I2C_BUS,     0x3C,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ug2864,      DEVHW_UG2864,       UG2864_I2C_BUS,     0x3C,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_PWM_SERVO_DRIVER)
@@ -322,8 +328,17 @@
         #if !defined(PCA9685_I2C_BUS)
             #define PCA9685_I2C_BUS BUS_I2C1
         #endif
-        BUSDEV_REGISTER_I2C(busdev_pca9685,      DEVHW_PCA9685,       PCA9685_I2C_BUS,     0x40,               NONE,           DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_pca9685,      DEVHW_PCA9685,       PCA9685_I2C_BUS,     0x40,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
+#if defined(USE_IRLOCK) && defined(USE_I2C)
+    #if !defined(IRLOCK_I2C_BUS) && defined(EXTERNAL_I2C_BUS)
+        #define IRLOCK_I2C_BUS EXTERNAL_I2C_BUS
+    #else
+        #define IRLOCK_I2C_BUS BUS_I2C1
+    #endif
+    BUSDEV_REGISTER_I2C(busdev_irlock,      DEVHW_IRLOCK,       IRLOCK_I2C_BUS,     0x54,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+#endif
+
 #endif  // USE_TARGET_HARDWARE_DESCRIPTORS
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index 92a0faf835..32b6e3531a 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -60,3 +60,20 @@
 // This feature uses 'arm_math.h', which does not exist for x86.
 #undef USE_DYNAMIC_FILTERS
 #endif
+
+//Defines for compiler optimizations
+#ifndef STM32F3
+#define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize("-Os")))
+#define FUNCTION_COMPILE_NORMAL __attribute__((optimize("-O2")))
+#define FUNCTION_COMPILE_FOR_SPEED __attribute__((optimize("-Ofast")))
+#define FILE_COMPILE_FOR_SIZE _Pragma("GCC optimize(\"Os\")")
+#define FILE_COMPILE_NORMAL _Pragma("GCC optimize(\"O2\")")
+#define FILE_COMPILE_FOR_SPEED _Pragma("GCC optimize(\"Ofast\")")
+#else
+#define FUNCTION_COMPILE_FOR_SIZE
+#define FUNCTION_COMPILE_NORMAL
+#define FUNCTION_COMPILE_FOR_SPEED
+#define FILE_COMPILE_FOR_SIZE
+#define FILE_COMPILE_NORMAL
+#define FILE_COMPILE_FOR_SPEED
+#endif
diff --git a/src/main/target/link/stm32_flash_f405.ld b/src/main/target/link/stm32_flash_f405.ld
index 4c728c5ebb..66721aee5f 100644
--- a/src/main/target/link/stm32_flash_f405.ld
+++ b/src/main/target/link/stm32_flash_f405.ld
@@ -37,4 +37,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", CCM)
 REGION_ALIAS("FASTRAM", CCM)
 
-INCLUDE "stm32_flash.ld"
\ No newline at end of file
+INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f427.ld b/src/main/target/link/stm32_flash_f427.ld
index 7cc7256337..8c080f38e7 100755
--- a/src/main/target/link/stm32_flash_f427.ld
+++ b/src/main/target/link/stm32_flash_f427.ld
@@ -15,7 +15,7 @@
 **
 **  Environment : Atollic TrueSTUDIO(R)
 **
-**  Distribution: The file is distributed �as is,� without any warranty
+**  Distribution: The file is distributed as is, without any warranty
 **                of any kind.
 **
 **  (c)Copyright Atollic AB.
@@ -56,4 +56,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", CCM)
 REGION_ALIAS("FASTRAM", CCM)
 
-INCLUDE "stm32_flash.ld"
\ No newline at end of file
+INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/sanity_check.h b/src/main/target/sanity_check.h
new file mode 100644
index 0000000000..8b78b89d4d
--- /dev/null
+++ b/src/main/target/sanity_check.h
@@ -0,0 +1,104 @@
+/*
+ * 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
+
+// Deprecated USE_GYRO/ACC defines
+#if defined (USE_GYRO) || defined (USE_ACC)
+#error "Unnecessary USE_ACC and/or USE_GYRO"
+#endif
+
+#if defined (USE_GYRO_MPU6000) || defined (USE_ACC_MPU6000) || defined (USE_GYRO_MPU6050) || defined (USE_ACC_MPU6050)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
+#if defined (USE_GYRO_MPU6500) || defined (USE_ACC_MPU6500) || defined (USE_GYRO_MPU9250) || defined (USE_ACC_MPU9250)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
+#if defined (USE_GYRO_ICM20689) || defined (USE_ACC_ICM20689)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
+#if defined (USE_FAKE_GYRO) || defined (USE_FAKE_ACC)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
+#if defined (USE_ACC_ADXL345) || defined (USE_ACC_LSM303DLHC) || defined (USE_ACC_MMA8452) || defined (USE_ACC_BMA280) || defined (USE_ACC_BMI160)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
+// Make sure IMU alignments are migrated to IMU_xxx_ALIGN
+#if defined (GYRO_MPU6050_ALIGN) || defined (ACC_MPU6050_ALIGN)
+#error "Replace GYRO_MPU6050_ALIGN and ACC_MPU6050_ALIGN with IMU_MPU6050_ALIGN"
+#endif
+
+#if defined (GYRO_MPU6000_ALIGN) || defined (ACC_MPU6000_ALIGN)
+#error "Replace GYRO_MPU6000_ALIGN and ACC_MPU6000_ALIGN with IMU_MPU6000_ALIGN"
+#endif
+
+#if defined (GYRO_MPU6500_ALIGN) || defined (ACC_MPU6500_ALIGN)
+#error "Replace GYRO_MPU6500_ALIGN and ACC_MPU6500_ALIGN with IMU_MPU6500_ALIGN"
+#endif
+
+#if defined (GYRO_MPU9250_ALIGN) || defined (ACC_MPU9250_ALIGN)
+#error "Replace GYRO_MPU9250_ALIGN and ACC_MPU9250_ALIGN with IMU_MPU9250_ALIGN"
+#endif
+
+#if defined (GYRO_BMI160_ALIGN) || defined (ACC_BMI160_ALIGN)
+#error "Replace GYRO_BMI160_ALIGN and ACC_BMI160_ALIGN with IMU_BMI160_ALIGN"
+#endif
+
+#if defined (GYRO_ICM20689_ALIGN) || defined (ACC_ICM20689_ALIGN)
+#error "Replace GYRO_ICM20689_ALIGN and ACC_ICM20689_ALIGN with IMU_ICM20689_ALIGN"
+#endif
+
+#if defined (GYRO_L3G4200D_ALIGN)
+#error "Replace GYRO_L3G4200D_ALIGN with IMU_L3G4200D_ALIGN"
+#endif
+
+#if defined (GYRO_MPU3050_ALIGN)
+#error "Replace GYRO_MPU3050_ALIGN with IMU_MPU3050_ALIGN"
+#endif
+
+#if defined (GYRO_L3GD20_ALIGN)
+#error "Replace GYRO_L3GD20_ALIGN with IMU_L3GD20_ALIGN"
+#endif
+
+#if defined (ACC_BMA280_ALIGN)
+#error "Replace ACC_BMA280_ALIGN with IMU_BMA280_ALIGN"
+#endif
+
+#if defined (ACC_MMA8452_ALIGN)
+#error "Replace ACC_MMA8452_ALIGN with IMU_MMA8452_ALIGN"
+#endif
+
+#if defined (ACC_ADXL345_ALIGN)
+#error "Replace ACC_ADXL345_ALIGN with IMU_ADXL345_ALIGN"
+#endif
+
+#if defined (ACC_LSM303DLHC_ALIGN)
+#error "Replace ACC_LSM303DLHC_ALIGN with IMU_LSM303DLHC_ALIGN"
+#endif
+
diff --git a/src/main/target/stm32f7xx_hal_conf.h b/src/main/target/stm32f7xx_hal_conf.h
index ebd30750b4..8c82cd3b95 100644
--- a/src/main/target/stm32f7xx_hal_conf.h
+++ b/src/main/target/stm32f7xx_hal_conf.h
@@ -69,7 +69,7 @@
 /* #define HAL_LTDC_MODULE_ENABLED   */
 /* #define HAL_QSPI_MODULE_ENABLED   */
 /* #define HAL_RNG_MODULE_ENABLED   */
-/* #define HAL_RTC_MODULE_ENABLED   */
+#define HAL_RTC_MODULE_ENABLED
 /* #define HAL_SAI_MODULE_ENABLED   */
 /* #define HAL_SD_MODULE_ENABLED   */
 /* #define HAL_MMC_MODULE_ENABLED   */
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 0e8c9dc2a5..4f8f210707 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -20,6 +20,7 @@
 #include <string.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_TELEMETRY) && defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
 
@@ -118,7 +119,7 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
             requestHandled |= sendMspReply(payloadSize, responseFn);
         }
         pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
-        ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
+        ATOMIC_BLOCK(NVIC_PRIO_SERIALUART) {
             if (pos >= mspRxBuffer.len) {
                 mspRxBuffer.len = 0;
                 return requestHandled;
diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c
index 457b760ba2..b551f6b9d1 100644
--- a/src/main/telemetry/frsky_d.c
+++ b/src/main/telemetry/frsky_d.c
@@ -196,7 +196,7 @@ static void sendThrottleOrBatterySizeAsRpm(void)
     uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
     sendDataHead(ID_RPM);
     if (ARMING_FLAG(ARMED)) {
-        const throttleStatus_e throttleStatus = calculateThrottleStatus();
+        const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
         if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
                     throttleForRPM = 0;
         serialize16(throttleForRPM);
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 9ac0dce545..dd980d242f 100755
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -9,6 +9,7 @@
 #include <math.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
 
@@ -43,6 +44,8 @@
 
 #include "navigation/navigation.h"
 
+#include "rx/frsky_crc.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
@@ -232,7 +235,7 @@ void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
     }
 
     if (checksum != NULL) {
-        *checksum += c;
+        frskyCheckSumStep(checksum, c);
     }
 }
 
@@ -247,8 +250,8 @@ void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *
     for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
         smartPortSendByte(*data++, &checksum, port);
     }
-    checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
-    smartPortSendByte((uint8_t)checksum, NULL, port);
+    frskyCheckSumFini(&checksum);
+    smartPortSendByte(checksum, NULL, port);
 }
 
 static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)
diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c
index 27dbd11c2e..4ab0d8dad4 100644
--- a/src/main/vcp/hw_config.c
+++ b/src/main/vcp/hw_config.c
@@ -194,23 +194,11 @@ void Leave_LowPowerMode(void)
  *******************************************************************************/
 void USB_Interrupts_Config(void)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
+    NVIC_SetPriority(USB_LP_CAN1_RX0_IRQn, NVIC_PRIO_USB);
+    NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn);
 
-    /* 2 bit for pre-emption priority, 2 bits for subpriority */
-    NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);     // is this really neccesary?
-
-    /* Enable the USB interrupt */
-    NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
-
-    /* Enable the USB Wake-up interrupt */
-    NVIC_InitStructure.NVIC_IRQChannel = USBWakeUp_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB_WUP);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB_WUP);
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(USBWakeUp_IRQn, NVIC_PRIO_USB);
+    NVIC_EnableIRQ(USBWakeUp_IRQn);
 }
 
 /*******************************************************************************
diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/vcp_hal/usbd_cdc_interface.c
index 26cf9e9c88..82207670af 100644
--- a/src/main/vcp_hal/usbd_cdc_interface.c
+++ b/src/main/vcp_hal/usbd_cdc_interface.c
@@ -284,6 +284,12 @@ static int8_t CDC_Itf_Receive(uint8_t* Buf, uint32_t *Len)
 {
     rxAvailable = *Len;
     rxBuffPtr = Buf;
+    if (!rxAvailable) {
+        // Received an empty packet, trigger receiving the next packet.
+        // This will happen after a packet that's exactly 64 bytes is received.
+        // The USB protocol requires that an empty (0 byte) packet immediately follow.
+        USBD_CDC_ReceivePacket(&USBD_Device);
+    }
     return (USBD_OK);
 }
 
diff --git a/src/main/vcpf4/usb_bsp.c b/src/main/vcpf4/usb_bsp.c
index 1160c47d8a..e322ec8931 100644
--- a/src/main/vcpf4/usb_bsp.c
+++ b/src/main/vcpf4/usb_bsp.c
@@ -49,23 +49,14 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
     (void)pdev;
     GPIO_InitTypeDef GPIO_InitStructure;
 
-#ifndef USE_ULPI_PHY
-#ifdef USB_OTG_FS_LOW_PWR_MGMT_SUPPORT
-    EXTI_InitTypeDef EXTI_InitStructure;
-    NVIC_InitTypeDef NVIC_InitStructure;
-#endif
-#endif
 
-    NVIC_InitTypeDef NVIC_InitStructure;
 #ifdef USE_USB_OTG_HS
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_IRQn;
+    NVIC_SetPriority(OTG_HS_IRQn, NVIC_PRIO_USB);
+    NVIC_DisableIRQ(OTG_HS_IRQn);
 #else
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_FS_IRQn;
+    NVIC_SetPriority(OTG_FS_IRQn, NVIC_PRIO_USB);
+    NVIC_DisableIRQ(OTG_FS_IRQn);
 #endif
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
-    NVIC_Init(&NVIC_InitStructure);
 
     RCC_AHB1PeriphClockCmd( RCC_AHB1Periph_GPIOA , ENABLE);
 
@@ -87,9 +78,6 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
     RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, ENABLE) ;
 
-    /* enable the PWR clock */
-    RCC_APB1PeriphResetCmd(RCC_APB1Periph_PWR, ENABLE);
-
     EXTI_ClearITPendingBit(EXTI_Line0);
 }
 /**
@@ -101,32 +89,13 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
 void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev)
 {
     (void)pdev;
-    NVIC_InitTypeDef NVIC_InitStructure;
 
-    NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
 #ifdef USE_USB_OTG_HS
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_IRQn;
+    NVIC_SetPriority(OTG_HS_IRQn, NVIC_PRIO_USB);
+    NVIC_EnableIRQ(OTG_HS_IRQn);
 #else
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_FS_IRQn;
-#endif
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
-#ifdef USB_OTG_HS_DEDICATED_EP1_ENABLED
-    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_EP1_OUT_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
-
-    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_EP1_IN_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(OTG_FS_IRQn, NVIC_PRIO_USB);
+    NVIC_EnableIRQ(OTG_FS_IRQn);
 #endif
 }
 /**
diff --git a/src/test/Makefile b/src/test/Makefile
index 444c5ba065..676bd496e7 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -701,24 +701,24 @@ $(OBJECT_DIR)/rcdevice_unittest : \
 
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
-$(OBJECT_DIR)/drivers/system.o : \
-	$(USER_DIR)/drivers/system.c \
-	$(USER_DIR)/drivers/system.h \
+$(OBJECT_DIR)/drivers/time.o : \
+	$(USER_DIR)/drivers/time.c \
+	$(USER_DIR)/drivers/time.h \
 	$(GTEST_HEADERS)
 
 	@mkdir -p $(dir $@)
-	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/system.c -o $@
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/time.c -o $@
 
 $(OBJECT_DIR)/time_unittest.o : \
 	$(TEST_DIR)/time_unittest.cc \
-	$(USER_DIR)/drivers/system.h \
+	$(USER_DIR)/drivers/time.h \
 	$(GTEST_HEADERS)
 
 	@mkdir -p $(dir $@)
 	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/time_unittest.cc -o $@
 
 $(OBJECT_DIR)/time_unittest : \
-	$(OBJECT_DIR)/drivers/system.o \
+	$(OBJECT_DIR)/drivers/time.o \
 	$(OBJECT_DIR)/time_unittest.o \
 	$(OBJECT_DIR)/gtest_main.a
 
diff --git a/src/test/unit/bitarray_unittest.cc b/src/test/unit/bitarray_unittest.cc
index 6375d828ae..d80f380d82 100644
--- a/src/test/unit/bitarray_unittest.cc
+++ b/src/test/unit/bitarray_unittest.cc
@@ -83,3 +83,14 @@ TEST(BitArrayTest, TestSetClrAll)
         EXPECT_EQ(ii, BITARRAY_FIND_FIRST_SET(p, ii));
     }
 }
+
+TEST(BitArrayTest, TestOutOfBounds)
+{
+    const int bits = 32 * 4;
+
+    BITARRAY_DECLARE(p, bits);
+    BITARRAY_CLR_ALL(p);
+    EXPECT_EQ(-1, BITARRAY_FIND_FIRST_SET(p, 0));
+    EXPECT_EQ(-1, BITARRAY_FIND_FIRST_SET(p, bits));
+    EXPECT_EQ(-1, BITARRAY_FIND_FIRST_SET(p, bits + 1));
+}
diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h
index a900257c42..4dcf2092c7 100644
--- a/src/test/unit/platform.h
+++ b/src/test/unit/platform.h
@@ -81,4 +81,10 @@ extern SysTick_Type *SysTick;
 
 #define FAST_CODE 
 #define NOINLINE
-#define EXTENDED_FASTRAM
\ No newline at end of file
+#define EXTENDED_FASTRAM
+#define FUNCTION_COMPILE_FOR_SIZE
+#define FUNCTION_COMPILE_NORMAL
+#define FUNCTION_COMPILE_FOR_SPEED
+#define FILE_COMPILE_FOR_SIZE
+#define FILE_COMPILE_NORMAL
+#define FILE_COMPILE_FOR_SPEED
\ No newline at end of file
diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc
index a17312e4dc..0e49751519 100644
--- a/src/test/unit/sensor_gyro_unittest.cc
+++ b/src/test/unit/sensor_gyro_unittest.cc
@@ -39,7 +39,7 @@ extern "C" {
     #include "sensors/sensors.h"
 
     extern zeroCalibrationVector_t gyroCalibration;
-    extern gyroDev_t gyroDev0;
+    extern gyroDev_t gyroDev[];
 
     STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware);
     STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration);
@@ -50,9 +50,8 @@ extern "C" {
 
 TEST(SensorGyro, Detect)
 {
-    const gyroSensor_e detected = gyroDetect(&gyroDev0, GYRO_AUTODETECT);
+    const gyroSensor_e detected = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
     EXPECT_EQ(GYRO_FAKE, detected);
-    EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
 }
 
 TEST(SensorGyro, Init)
@@ -67,11 +66,11 @@ TEST(SensorGyro, Read)
     gyroInit();
     EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
     fakeGyroSet(5, 6, 7);
-    const bool read = gyroDev0.readFn(&gyroDev0);
+    const bool read = gyroDev[0].readFn(&gyroDev[0]);
     EXPECT_EQ(true, read);
-    EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]);
-    EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]);
-    EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]);
+    EXPECT_EQ(5, gyroDev[0].gyroADCRaw[X]);
+    EXPECT_EQ(6, gyroDev[0].gyroADCRaw[Y]);
+    EXPECT_EQ(7, gyroDev[0].gyroADCRaw[Z]);
 }
 
 TEST(SensorGyro, Calibrate)
@@ -79,25 +78,25 @@ TEST(SensorGyro, Calibrate)
     gyroStartCalibration();
     gyroInit();
     fakeGyroSet(5, 6, 7);
-    const bool read = gyroDev0.readFn(&gyroDev0);
+    const bool read = gyroDev[0].readFn(&gyroDev[0]);
     EXPECT_EQ(true, read);
-    EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]);
-    EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]);
-    EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]);
-    gyroDev0.gyroZero[X] = 8;
-    gyroDev0.gyroZero[Y] = 9;
-    gyroDev0.gyroZero[Z] = 10;
-    performGyroCalibration(&gyroDev0, &gyroCalibration);
-    EXPECT_EQ(0, gyroDev0.gyroZero[X]);
-    EXPECT_EQ(0, gyroDev0.gyroZero[Y]);
-    EXPECT_EQ(0, gyroDev0.gyroZero[Z]);
+    EXPECT_EQ(5, gyroDev[0].gyroADCRaw[X]);
+    EXPECT_EQ(6, gyroDev[0].gyroADCRaw[Y]);
+    EXPECT_EQ(7, gyroDev[0].gyroADCRaw[Z]);
+    gyroDev[0].gyroZero[X] = 8;
+    gyroDev[0].gyroZero[Y] = 9;
+    gyroDev[0].gyroZero[Z] = 10;
+    performGyroCalibration(&gyroDev[0], &gyroCalibration);
+    EXPECT_EQ(0, gyroDev[0].gyroZero[X]);
+    EXPECT_EQ(0, gyroDev[0].gyroZero[Y]);
+    EXPECT_EQ(0, gyroDev[0].gyroZero[Z]);
     EXPECT_EQ(false, gyroIsCalibrationComplete());
     while (!gyroIsCalibrationComplete()) {
-        performGyroCalibration(&gyroDev0, &gyroCalibration);
+        performGyroCalibration(&gyroDev[0], &gyroCalibration);
     }
-    EXPECT_EQ(5, gyroDev0.gyroZero[X]);
-    EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
-    EXPECT_EQ(7, gyroDev0.gyroZero[Z]);
+    EXPECT_EQ(5, gyroDev[0].gyroZero[X]);
+    EXPECT_EQ(6, gyroDev[0].gyroZero[Y]);
+    EXPECT_EQ(7, gyroDev[0].gyroZero[Z]);
 }
 
 TEST(SensorGyro, Update)
@@ -112,9 +111,9 @@ TEST(SensorGyro, Update)
         gyroUpdate();
     }
     EXPECT_EQ(true, gyroIsCalibrationComplete());
-    EXPECT_EQ(5, gyroDev0.gyroZero[X]);
-    EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
-    EXPECT_EQ(7, gyroDev0.gyroZero[Z]);
+    EXPECT_EQ(5, gyroDev[0].gyroZero[X]);
+    EXPECT_EQ(6, gyroDev[0].gyroZero[Y]);
+    EXPECT_EQ(7, gyroDev[0].gyroZero[Z]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
@@ -125,9 +124,9 @@ TEST(SensorGyro, Update)
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
     fakeGyroSet(15, 26, 97);
     gyroUpdate();
-    EXPECT_FLOAT_EQ(10 * gyroDev0.scale, gyro.gyroADCf[X]); // gyroADCf values are scaled
-    EXPECT_FLOAT_EQ(20 * gyroDev0.scale, gyro.gyroADCf[Y]);
-    EXPECT_FLOAT_EQ(90 * gyroDev0.scale, gyro.gyroADCf[Z]);
+    EXPECT_FLOAT_EQ(10 * gyroDev[0].scale, gyro.gyroADCf[X]); // gyroADCf values are scaled
+    EXPECT_FLOAT_EQ(20 * gyroDev[0].scale, gyro.gyroADCf[Y]);
+    EXPECT_FLOAT_EQ(90 * gyroDev[0].scale, gyro.gyroADCf[Z]);
 }
 
 
diff --git a/src/test/unit/target.h b/src/test/unit/target.h
index 23987c9aab..ad0ad7a21d 100755
--- a/src/test/unit/target.h
+++ b/src/test/unit/target.h
@@ -35,7 +35,7 @@
 #define USE_TELEMETRY_LTM
 #define USE_LED_STRIP
 #define TRANSPONDER
-#define USE_FAKE_GYRO
+#define USE_IMU_FAKE
 #define USE_VCP
 #define USE_UART1
 #define USE_UART2
diff --git a/src/utils/build-targets.sh b/src/utils/build-targets.sh
new file mode 100755
index 0000000000..f8633819bf
--- /dev/null
+++ b/src/utils/build-targets.sh
@@ -0,0 +1,70 @@
+#!/bin/sh
+
+set -e
+
+usage()
+{
+   echo "Usage ${0} [-n] [-S suffix] -s START -c COUNT]" >&2;
+   echo "Build COUNT targets starting at index START" >&2;
+   echo "Targets are retrieved via make targets" >&2;
+   echo "-n prints the targets instead of building them" >&2;
+   echo "-S sets the BUILD_SUFFIX" >&2;
+   exit 1;
+}
+
+is_number()
+{
+    ! [ -z ${1} ] && [ ${1} -eq ${1} ] 2>/dev/null
+}
+
+START=
+COUNT=
+DRY_RUN=
+BUILD_SUFFIX=
+
+args=`getopt nS:s:c: $*`
+set -- $args
+for i
+do
+    case "$i"
+    in
+        -n)
+            DRY_RUN=1
+            shift;;
+        -S)
+            BUILD_SUFFIX=${2}; shift;
+            shift;;
+        -s)
+            START=${2}; shift;
+            shift;;
+        -c)
+            COUNT=${2}; shift;
+            shift;;
+        --)
+            shift; break;;
+    esac
+done
+
+if ! is_number ${START} || ! is_number ${COUNT}; then
+    usage;
+fi
+
+ALL_TARGETS=$(make targets | grep Valid | awk -F' *: *' '{print $2}')
+START_IDX=$(expr ${START} + 1)
+END_IDX=$(expr ${START} + ${COUNT})
+SELECTED_TARGETS=$(echo ${ALL_TARGETS} | cut -d ' ' -f ${START_IDX}-${END_IDX})
+
+if [ -z "${DRY_RUN}" ]; then
+    for target in ${SELECTED_TARGETS}; do
+        BUILD_SUFFIX=${BUILD_SUFFIX} V=0 CFLAGS=-Werror make ${target}
+        # Cleanup intermediate files after building each target.
+        # Otherwise we run out of space on GH CI.
+        # XXX: Make sure we save the .hex file for artifact
+        # generation
+        mkdir -p obj/dist
+        mv obj/*.hex obj/dist
+        V=0 make clean_${target}
+    done
+else
+    echo "${SELECTED_TARGETS}"
+fi