Merge remote-tracking branch 'upstream/master' into abo_multi_function_utility

2025-07-26 17:55:28 +03:00 · 2023-08-28 12:59:52 +01:00 · 2023-08-28 12:59:52 +01:00 · 274bfe6e69
commit 274bfe6e69
parent 5fde0a041d 029ae30258
250 changed files with 7806 additions and 2840 deletions
--- a/.dir-locals.el
+++ b/.dir-locals.el
@ -0,0 +1,5 @@
+;;; Directory Local Variables            -*- no-byte-compile: t -*-
+;;; For more information see (info "(emacs) Directory Variables")
+
+((nil . ((c-basic-offset . 4)
+	 (c-default-style . "k&r"))))
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@ -38,9 +38,9 @@ jobs:
      - name: Build targets (${{ matrix.id }})
        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
      - name: Upload artifacts
-        uses: actions/upload-artifact@v2-preview
+        uses: actions/upload-artifact@v3
        with:
-          name: ${{ env.BUILD_NAME }}.zip
+          name: ${{ env.BUILD_NAME }}
          path: ./build/*.hex

  build-SITL-Linux:
@ -68,9 +68,45 @@ jobs:
      - name: Build SITL
        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
      - name: Upload artifacts
-        uses: actions/upload-artifact@v2-preview
+        uses: actions/upload-artifact@v3
        with:
-          name: ${{ env.BUILD_NAME }}_SITL.zip
+          name: ${{ env.BUILD_NAME }}_SITL
+          path: ./build_SITL/*_SITL
+
+  build-SITL-Mac:
+    runs-on: macos-latest
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: |
+          brew install cmake ninja ruby
+
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        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})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: |
+          mkdir -p build_SITL && cd build_SITL
+          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
+          ninja
+
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL-MacOS
          path: ./build_SITL/*_SITL

  build-SITL-Windows:
@ -103,9 +139,9 @@ jobs:
      - name: Build SITL
        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
      - name: Upload artifacts
-        uses: actions/upload-artifact@v2-preview
+        uses: actions/upload-artifact@v3
        with:
-          name: ${{ env.BUILD_NAME }}_SITL-WIN.zip
+          name: ${{ env.BUILD_NAME }}_SITL-WIN
          path: ./build_SITL/*.exe


--- a/.gitignore
+++ b/.gitignore
@ -12,7 +12,7 @@
 __pycache__
 startup_stm32f10x_md_gcc.s
 .vagrant/
-.vscode/
+#.vscode/
 cov-int*
 /build/
 /obj/
@ -31,3 +31,4 @@ README.pdf

 # local changes only
 make/local.mk
+launch.json
--- a/.vimrc
+++ b/.vimrc
@ -0,0 +1,9 @@
+filetype on
+filetype indent on
+
+set expandtab
+set bs=2
+set sw=4
+set ts=4
+
+
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@ -0,0 +1,12 @@
+{
+    "files.associations": {
+        "chrono": "c",
+        "cmath": "c",
+        "ranges": "c"
+    },
+    "editor.tabSize": 4,
+    "editor.insertSpaces": true,
+    "editor.detectIndentation": false,
+    "editor.expandTabs": true,
+    "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4,  BreakBeforeBraces: Mozilla }"
+}
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -14,6 +14,9 @@ option(SITL "SITL build for host system" OFF)

 set(TOOLCHAIN_OPTIONS none arm-none-eabi host)
 if (SITL)
+    if (CMAKE_HOST_APPLE)
+        set(MACOSX TRUE)
+    endif()
    set(TOOLCHAIN "host" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}")
 else()
    set(TOOLCHAIN "arm-none-eabi" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}")
@ -52,7 +55,11 @@ project(INAV VERSION 7.0.0)

 enable_language(ASM)

-set(CMAKE_C_STANDARD 99)
+if(MACOSX AND SITL)
+    set(CMAKE_C_STANDARD 11)
+else()
+    set(CMAKE_C_STANDARD 99)
+endif()
 set(CMAKE_C_EXTENSIONS ON)
 set(CMAKE_C_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_STANDARD 11)
--- a/cmake/at32.cmake
+++ b/cmake/at32.cmake
@ -15,6 +15,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP")
 set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include")

 set(CMSIS_DSP_SRC
+    BasicMathFunctions/arm_scale_f32.c
+    BasicMathFunctions/arm_sub_f32.c
    BasicMathFunctions/arm_mult_f32.c
    TransformFunctions/arm_rfft_fast_f32.c
    TransformFunctions/arm_cfft_f32.c
--- a/cmake/host.cmake
+++ b/cmake/host.cmake
@ -16,7 +16,11 @@ set(CMAKE_CXX_COMPILER "g++${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "c++ compil
 set(CMAKE_OBJCOPY "objcopy${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "objcopy tool")
 set(CMAKE_OBJDUMP "objdump${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "objdump tool")
 set(CMAKE_SIZE "size${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "size tool")
-set(CMAKE_DEBUGGER "gdb${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "debugger")
+if(CMAKE_HOST_APPLE)
+  set(CMAKE_DEBUGGER "lldb${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "debugger")
+else()
+  set(CMAKE_DEBUGGER "gdb${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "debugger")
+endif()
 set(CMAKE_CPPFILT "c++filt${TOOL_EXECUTABLE_SUFFIX}" CACHE INTERNAL "c++filt")

 set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "Build Type" FORCE)
@ -24,7 +28,11 @@ set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${CMAKE_CONFIGURATION_TYPES

 set(debug_options "-Og -O0 -g")
 set(release_options "-Os -DNDEBUG")
-set(relwithdebinfo_options "-ggdb3 ${release_options}")
+if(CMAKE_HOST_APPLE)
+  set(relwithdebinfo_options "-g ${release_options}")
+else()
+  set(relwithdebinfo_options "-ggdb3 ${release_options}")
+endif()

 set(CMAKE_C_FLAGS_DEBUG ${debug_options} CACHE INTERNAL "c compiler flags debug")
 set(CMAKE_CXX_FLAGS_DEBUG ${debug_options} CACHE INTERNAL "c++ compiler flags debug")
--- a/cmake/main.cmake
+++ b/cmake/main.cmake
@ -9,14 +9,27 @@ set(MAIN_DEFINITIONS
    __REVISION__="${GIT_REV}"
 )

-set(MAIN_COMPILE_OPTIONS
-    -Wall
-    -Wextra
-    -Wunsafe-loop-optimizations
-    -Wdouble-promotion
-    -Wstrict-prototypes
-    -Werror=switch
-)
+
+# Can't check for OSX yet at this point
+if(SITL)
+    set(MAIN_COMPILE_OPTIONS
+        -Wall
+        -Wextra
+        -Wdouble-promotion
+        -Wstrict-prototypes
+        -Werror=switch
+        #-Wno-unknown-warning-option
+    )
+else()
+    set(MAIN_COMPILE_OPTIONS
+        -Wall
+        -Wextra
+        -Wunsafe-loop-optimizations
+        -Wdouble-promotion
+        -Wstrict-prototypes
+        -Werror=switch
+    )
+endif()

 macro(main_sources var) # list-var src-1...src-n
    set(${var} ${ARGN})
@ -70,7 +83,7 @@ function(setup_executable exe name)
    set_target_properties(${exe} PROPERTIES
        RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin"
    )
-    if(IS_RELEASE_BUILD)
+    if(IS_RELEASE_BUILD AND NOT (CMAKE_HOST_APPLE AND SITL))
        set_target_properties(${exe} PROPERTIES
            INTERPROCEDURAL_OPTIMIZATION ON
        )
--- a/cmake/sitl.cmake
+++ b/cmake/sitl.cmake
@ -25,41 +25,56 @@ main_sources(SITL_SRC
    target/SITL/sim/xplane.h
 )

+
+if(CMAKE_HOST_APPLE)
+  set(MACOSX ON)
+endif()
+
 set(SITL_LINK_OPTIONS
-    -lrt
    -Wl,-L${STM32_LINKER_DIR}
-    -Wl,--cref
-    -static-libgcc # Required for windows build under cygwin
 )

+if(${WIN32} OR ${CYGWIN})
+    set(SITL_LINK_OPTIONS ${SITL_LINK_OPTIONS} "-static-libgcc")
+endif()
+
 set(SITL_LINK_LIBRARIS
    -lpthread
    -lm
    -lc
 )

+if(NOT MACOSX)
+    set(SITL_LINK_LIBRARIS ${SITL_LINK_LIBRARIS} -lrt)
+endif()
+
 set(SITL_COMPILE_OPTIONS
    -Wno-format #Fixme: Compile for 32bit, but settings.rb has to be adjusted
-    -Wno-return-local-addr
-    -Wno-error=maybe-uninitialized
-    -fsingle-precision-constant
    -funsigned-char
 )

+if(NOT MACOSX)
+    set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS}
+        -Wno-return-local-addr
+        -Wno-error=maybe-uninitialized
+        -fsingle-precision-constant
+    )
+    if (CMAKE_COMPILER_IS_GNUCC AND NOT CMAKE_C_COMPILER_VERSION VERSION_LESS 12.0)
+        set(SITL_LINK_OPTIONS ${SITL_LINK_OPTIONS} "-Wl,--no-warn-rwx-segments")
+    endif()
+else()
+    set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS}
+    )
+endif()
+
 set(SITL_DEFINITIONS
    SITL_BUILD
 )

-function(generate_map_file target)
-    if(CMAKE_VERSION VERSION_LESS 3.15)
-        set(map "$<TARGET_FILE:${target}>.map")
-    else()
-        set(map "$<TARGET_FILE_DIR:${target}>/$<TARGET_FILE_BASE_NAME:${target}>.map")
-    endif()
-    target_link_options(${target} PRIVATE "-Wl,-gc-sections,-Map,${map}")
-endfunction()
-
 function (target_sitl name)
+    if(CMAKE_VERSION VERSION_GREATER 3.22)
+        set(CMAKE_C_STANDARD 17)
+    endif()

    if(NOT host STREQUAL TOOLCHAIN)
        return()
@ -102,14 +117,14 @@ function (target_sitl name)
    target_link_libraries(${exe_target} PRIVATE ${SITL_LINK_LIBRARIS})
    target_link_options(${exe_target} PRIVATE ${SITL_LINK_OPTIONS})

-    generate_map_file(${exe_target})
-    
    set(script_path ${MAIN_SRC_DIR}/target/link/sitl.ld)
    if(NOT EXISTS ${script_path})
        message(FATAL_ERROR "linker script ${script_path} doesn't exist")
    endif()
    set_target_properties(${exe_target} PROPERTIES LINK_DEPENDS ${script_path})
-    target_link_options(${exe_target} PRIVATE -T${script_path})
+    if(NOT MACOSX)
+        target_link_options(${exe_target} PRIVATE -T${script_path})
+    endif()

    if(${WIN32} OR ${CYGWIN})
        set(exe_filename ${CMAKE_BINARY_DIR}/${binary_name}.exe)
@ -118,9 +133,7 @@ function (target_sitl name)
    endif()

    add_custom_target(${name} ALL
-        cmake -E env PATH="$ENV{PATH}"
-        ${CMAKE_OBJCOPY} $<TARGET_FILE:${exe_target}> ${exe_filename}
-        BYPRODUCTS ${hex}
+        cmake -E copy $<TARGET_FILE:${exe_target}> ${exe_filename}
    )

    setup_firmware_target(${exe_target} ${name} ${ARGN})
--- a/cmake/stm32.cmake
+++ b/cmake/stm32.cmake
@ -16,6 +16,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP")
 set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include")

 set(CMSIS_DSP_SRC
+    BasicMathFunctions/arm_scale_f32.c
+    BasicMathFunctions/arm_sub_f32.c
    BasicMathFunctions/arm_mult_f32.c
    TransformFunctions/arm_rfft_fast_f32.c
    TransformFunctions/arm_cfft_f32.c
--- a/cmake/stm32h7.cmake
+++ b/cmake/stm32h7.cmake
@ -149,6 +149,8 @@ main_sources(STM32H7_SRC
    drivers/bus_i2c_hal.c
    drivers/dma_stm32h7xx.c
    drivers/bus_spi_hal_ll.c
+    drivers/bus_quadspi.c
+    drivers/bus_quadspi_hal.c
    drivers/memprot.h
    drivers/memprot_hal.c
    drivers/memprot_stm32h7xx.c
@ -185,7 +187,7 @@ function(target_stm32h7xx)
        VCP_SOURCES ${STM32H7_USB_SRC} ${STM32H7_VCP_SRC}
        VCP_INCLUDE_DIRECTORIES ${STM32H7_USB_INCLUDE_DIRS} ${STM32H7_VCP_DIR}

-        OPTIMIZATION -Ofast
+        OPTIMIZATION -O2

        OPENOCD_TARGET stm32h7x

--- a/docs/Buzzer.md
+++ b/docs/Buzzer.md
@ -62,12 +62,14 @@ beeper list
 giving:

 ```
-Available:  RUNTIME_CALIBRATION  HW_FAILURE  RX_LOST  RX_LOST_LANDING  DISARMING  ARMING  ARMING_GPS_FIX  BAT_CRIT_LOW  BAT_LOW  GPS_STATUS  RX_SET  ACTION_SUCCESS  ACTION_FAIL  READY_BEEP  MULTI_BEEPS  DISARM_REPEAT  ARMED  SYSTEM_INIT  ON_USB  LAUNCH_MODE  CAM_CONNECTION_OPEN  CAM_CONNECTION_CLOSED  ALL  PREFERED
+Available:  RUNTIME_CALIBRATION  HW_FAILURE  RX_LOST  RX_LOST_LANDING  DISARMING  ARMING  ARMING_GPS_FIX  BAT_CRIT_LOW
+BAT_LOW  GPS_STATUS  RX_SET  ACTION_SUCCESS  ACTION_FAIL  READY_BEEP  MULTI_BEEPS  DISARM_REPEAT  ARMED  SYSTEM_INIT
+ON_USB LAUNCH_MODE  CAM_CONNECTION_OPEN  CAM_CONNECTION_CLOSED  ALL  PREFERED
 ```

 The `beeper` command  syntax follows that of the `feature` command; a minus (`-`) in front of a name disables that function.

-So to disable the beeper / buzzer when connected to USB (may enhance domestic harmony)
+So to disable the beeper / buzzer when 	powered by USB (may enhance domestic harmony):

 ```
 beeper -ON_USB
@ -78,17 +80,39 @@ Now the `beeper` command will show:
 ```
 # beeper
 Disabled:  ON_USB
+```
+
+*Note: SYSTEM_INIT sequence is not affected by ON_USB setting and will still be played on USB connection. Disable both ON_USB and SYSTEM_INIT to disable buzzer completely when FC is powered from USB.*
+
+*Note: ON_USB setting requires present and configured battery voltage metter.*
+
+To disable all features use:

 ```
+beeper -ALL
+```
+
+To store current set to preferences use (preferences also require ```save```):
+
+```
+beeper PREFERED
+```
+
+To restore set from preferences use:
+
+```
+beeper -PREFERED

 As with other CLI commands, the `save` command is needed to save the new settings.

 ## Types of buzzer supported

-The buzzers are enabled/disabled by simply enabling or disabling a GPIO output pin on the board.
+Most FCs require ACTIVE buzzers. Active buzzers are enabled/disabled by simply enabling or disabling a GPIO output pin on the board.
 This means the buzzer must be able to generate its own tone simply by having power applied to it.

-Buzzers that need an analog or PWM signal do not work and will make clicking noises or no sound at all.
+Passive buzzers that need an analog or PWM signal do not work and will make clicking noises or no sound at all.
+
+Passive buzzers are supported on FCs which are designed to work with passive buzzers only (so far there is no available, except rare cases like Matek F765-WSE where passive buzzer is preinstalled).

 Examples of a known-working buzzers.

--- a/docs/Controls.md
+++ b/docs/Controls.md
@ -44,6 +44,10 @@ The stick positions are combined to activate different functions:
 | Bypass Nav Arm disable        | LOW      | HIGH    | CENTER | CENTER |
 | Save setting                  | LOW      | LOW     | LOW    | HIGH   |
 | Enter OSD Menu (CMS)          | CENTER   | LOW     | HIGH   | CENTER |
+| Enter Camera OSD(RuncamDevice)| RIGHT    | CENTER  | CENTER | CENTER |
+| Exit Camera OSD (RuncamDevice)| LEFT     | CENTER  | CENTER | CENTER |
+| Confirm - Camera OSD          | RIGHT    | CENTER  | CENTER | CENTER |
+| Navigation - Camera OSD       | CENTER   | CENTER  | *      | *      |

 For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/).
 ![Stick Positions](assets/images/StickPositions.png)
--- a/docs/LedStrip.md
+++ b/docs/LedStrip.md
@ -237,6 +237,12 @@ The mapping between modes led placement and colors is currently fixed and cannot

 #### Indicator

+##### For fixed wing (INAV 6.1 onwards)
+
+This mode flashes LEDs that correspond to the roll stick position. Rolling left will flash any `indicator` LED on the left half of the grid. Rolling right will flash any `indicator` on the right side of the grid.
+
+##### For other platforms (all platforms pre INAV 6.1)
+
 This mode flashes LEDs that correspond to roll and pitch stick positions.  i.e.  they indicate the direction the craft is going to turn.

 | Mode | Direction | LED Color |
--- a/docs/Navigation.md
+++ b/docs/Navigation.md
@ -4,11 +4,14 @@ The navigation system in INAV is responsible for assisting the pilot allowing al

 ## NAV ALTHOLD mode - altitude hold

-Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically. GPS is available as an altitude source for airplanes only.
+Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically. 
 In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - quad goes up, pilot moves stick down - quad descends, you keep stick at neutral position - quad hovers.

+By default, GPS is available as an altitude source for airplanes only. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled.
+
 ### CLI parameters affecting ALTHOLD mode:
 * *nav_use_midthr_for_althold* - when set to "0", firmware will remember where your throttle stick was when ALTHOLD was activated - this will be considered neutral position. When set to "1" - 50% throttle will be considered neutral position.
+* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. 

 ### Related PIDs
 PIDs affecting altitude hold: ALT & VEL
@ -22,11 +25,12 @@ 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.
+Position hold requires GPS, accelerometer and compass sensors. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled. 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). From INAV 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically.

 ### CLI parameters affecting POSHOLD 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.
+* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. 
 
 ### Related PIDs
 PIDs affecting position hold: POS, POSR
@ -38,9 +42,9 @@ PID meaning:

 Home for RTH is the position where vehicle was first armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. This position may also be overridden with Safehomes. 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.
+RTH requires barometer for multirotor, unless unless *inav_use_gps_no_baro* is enabled.

-If barometer is present, RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings.
+RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings.
 When deciding what altitude to maintain, RTH has 6 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables):
 * 0 (NAV_RTH_NO_ALT) - keep current altitude during whole RTH sequence (*nav_rth_altitude* is ignored)
 * 1 (NAV_RTH_EXTRA_ALT) - climb to current altitude plus extra margin prior to heading home (*nav_rth_altitude* defines the extra altitude (cm))
--- a/docs/OSD.md
+++ b/docs/OSD.md
@ -0,0 +1,167 @@
+# On Screen Display
+
+The On Screen Display, or OSD, is a feature that overlays flight data over the video image. This can be done on the flight controller, using the analogue MAX7456 chip. Digital systems take the OSD data, via MSP DisplayPort, send it to the video receiver; which combines the data with the image. You can specify what elements are displayed, and their locations on the image. Most systems are character based, and use the MAX7456 analogue setup, or MSP DisplayPort. However, there are some different systems which are also supported. Such as the canvas based FrSKY PixelOSD on analogue. Canvas OSDs draw shapes on the image. Whereas character based OSDs use font characters to display the data.
+
+## Features and Limitations
+Not all OSDs are created equally. This table shows the differences between the different systems available.
+
+| OSD System	| Character grid	| Character	| Canvas	| MSP DisplayPort	| All elements supported	|
+|---------------|-------------------|-----------|-----------|-------------------|---------------------------|
+| Analogue PAL	| 30 x 16			| X			|			|					| YES						|
+| Analogue NTSC	| 30 x 13			| X			|			|					| YES						|
+| PixelOSD		| As PAL or NTSC	|			| X			|					| YES						|
+| DJI OSD		| 30 x 16			| X			|			|					| NO - BF Characters only	|
+| DJI WTFOS		| 60 x 22			| X			|			| X					| YES						|
+| HDZero		| 50 x 18			| X			|			| X					| YES						|
+| Avatar		| 53 x 20			| X			|			| X					| YES						|
+| DJI O3		| 53 x 20 (HD)		| X			|			| X	(partial)		| NO - BF Characters only	|
+
+## OSD Elements
+Here are the OSD Elements provided by INAV.
+
+| ID	| Element										| Added |
+|-------|-----------------------------------------------|-------|
+| 0		| OSD_RSSI_VALUE								| 1.0.0	|
+| 1		| OSD_MAIN_BATT_VOLTAGE							| 1.0.0	|
+| 2		| OSD_CROSSHAIRS								| 1.0.0	|
+| 3		| OSD_ARTIFICIAL_HORIZON						| 1.0.0	|
+| 4		| OSD_HORIZON_SIDEBARS							| 1.0.0	|
+| 5		| OSD_ONTIME									| 1.0.0	|
+| 6		| OSD_FLYTIME									| 1.0.0	|
+| 7		| OSD_FLYMODE									| 1.0.0	|
+| 8		| OSD_CRAFT_NAME								| 1.0.0	|
+| 9		| OSD_THROTTLE_POS								| 1.0.0	|
+| 10	| OSD_VTX_CHANNEL								| 1.0.0	|
+| 11	| OSD_CURRENT_DRAW								| 1.0.0	|
+| 12	| OSD_MAH_DRAWN									| 1.0.0	|
+| 13	| OSD_GPS_SPEED									| 1.0.0	|
+| 14	| OSD_GPS_SATS									| 1.0.0	|
+| 15	| OSD_ALTITUDE									| 1.0.0	|
+| 16	| OSD_ROLL_PIDS									| 1.6.0	|
+| 17	| OSD_PITCH_PIDS								| 1.6.0	|
+| 18	| OSD_YAW_PIDS									| 1.6.0	|
+| 19	| OSD_POWER										| 1.6.0	|
+| 20	| OSD_GPS_LON									| 1.6.0	|
+| 21	| OSD_GPS_LAT									| 1.6.0	|
+| 22	| OSD_HOME_DIR									| 1.6.0	|
+| 23	| OSD_HOME_DIST									| 1.6.0	|
+| 24	| OSD_HEADING									| 1.6.0	|
+| 25	| OSD_VARIO										| 1.6.0	|
+| 26	| OSD_VARIO_NUM									| 1.6.0	|
+| 27	| OSD_AIR_SPEED									| 1.7.3	|
+| 28	| OSD_ONTIME_FLYTIME							| 1.8.0	|
+| 29	| OSD_RTC_TIME									| 1.8.0	|
+| 30	| OSD_MESSAGES									| 1.8.0	|
+| 31	| OSD_GPS_HDOP									| 1.8.0	|
+| 32	| OSD_MAIN_BATT_CELL_VOLTAGE					| 1.8.0	|
+| 33	| OSD_SCALED_THROTTLE_POS						| 1.8.0	|
+| 34	| OSD_HEADING_GRAPH								| 1.8.0	|
+| 35	| OSD_EFFICIENCY_MAH_PER_KM						| 1.9.0	|
+| 36	| OSD_WH_DRAWN									| 1.9.0	|
+| 37	| OSD_BATTERY_REMAINING_CAPACITY				| 1.9.0	|
+| 38	| OSD_BATTERY_REMAINING_PERCENT					| 1.9.0	|
+| 39	| OSD_EFFICIENCY_WH_PER_KM						| 1.9.0	|
+| 40	| OSD_TRIP_DIST									| 1.9.1	|
+| 41	| OSD_ATTITUDE_PITCH							| 2.0.0	|
+| 42	| OSD_ATTITUDE_ROLL								| 2.0.0	|
+| 43	| OSD_MAP_NORTH									| 2.0.0	|
+| 44	| OSD_MAP_TAKEOFF								| 2.0.0	|
+| 45	| OSD_RADAR										| 2.0.0	|
+| 46	| OSD_WIND_SPEED_HORIZONTAL						| 2.0.0	|
+| 47	| OSD_WIND_SPEED_VERTICAL						| 2.0.0	|
+| 48	| OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH			| 2.0.0	|
+| 49	| OSD_REMAINING_DISTANCE_BEFORE_RTH				| 2.0.0	|
+| 50	| OSD_HOME_HEADING_ERROR						| 2.0.0	|
+| 51	| OSD_COURSE_HOLD_ERROR							| 2.0.0	|
+| 52	| OSD_COURSE_HOLD_ADJUSTMENT					| 2.0.0	|
+| 53	| OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE			| 2.0.0	|
+| 54	| OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE	| 2.0.0	|
+| 55	| OSD_POWER_SUPPLY_IMPEDANCE					| 2.0.0	|
+| 56	| OSD_LEVEL_PIDS								| 2.0.0	|
+| 57	| OSD_POS_XY_PIDS								| 2.0.0	|
+| 58	| OSD_POS_Z_PIDS								| 2.0.0	|
+| 59	| OSD_VEL_XY_PIDS								| 2.0.0	|
+| 60	| OSD_VEL_Z_PIDS								| 2.0.0	|
+| 61	| OSD_HEADING_P									| 2.0.0	|
+| 62	| OSD_BOARD_ALIGN_ROLL							| 2.0.0	|
+| 63	| OSD_BOARD_ALIGN_PITCH							| 2.0.0	|
+| 64	| OSD_RC_EXPO									| 2.0.0	|
+| 65	| OSD_RC_YAW_EXPO								| 2.0.0	|
+| 66	| OSD_THROTTLE_EXPO								| 2.0.0	|
+| 67	| OSD_PITCH_RATE								| 2.0.0	|
+| 68	| OSD_ROLL_RATE									| 2.0.0	|
+| 69	| OSD_YAW_RATE									| 2.0.0	|
+| 70	| OSD_MANUAL_RC_EXPO							| 2.0.0	|
+| 71	| OSD_MANUAL_RC_YAW_EXPO						| 2.0.0	|
+| 72	| OSD_MANUAL_PITCH_RATE							| 2.0.0	|
+| 73	| OSD_MANUAL_ROLL_RATE							| 2.0.0	|
+| 74	| OSD_MANUAL_YAW_RATE							| 2.0.0	|
+| 75	| OSD_NAV_FW_CRUISE_THR							| 2.0.0	|
+| 76	| OSD_NAV_FW_PITCH2THR							| 2.0.0	|
+| 77	| OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE			| 2.0.0	|
+| 78	| OSD_DEBUG										| 2.0.0	|
+| 79	| OSD_FW_ALT_PID_OUTPUTS						| 2.0.0	|
+| 80	| OSD_FW_POS_PID_OUTPUTS						| 2.0.0	|
+| 81	| OSD_MC_VEL_X_PID_OUTPUTS						| 2.0.0	|
+| 82	| OSD_MC_VEL_Y_PID_OUTPUTS						| 2.0.0	|
+| 83	| OSD_MC_VEL_Z_PID_OUTPUTS						| 2.0.0	|
+| 84	| OSD_MC_POS_XYZ_P_OUTPUTS						| 2.0.0	|
+| 85	| OSD_3D_SPEED									| 2.1.0	|
+| 86	| OSD_IMU_TEMPERATURE							| 2.1.0	|
+| 87	| OSD_BARO_TEMPERATURE							| 2.1.0	|
+| 88	| OSD_TEMP_SENSOR_0_TEMPERATURE					| 2.1.0	|
+| 89	| OSD_TEMP_SENSOR_1_TEMPERATURE					| 2.1.0	|
+| 90	| OSD_TEMP_SENSOR_2_TEMPERATURE					| 2.1.0	|
+| 91	| OSD_TEMP_SENSOR_3_TEMPERATURE					| 2.1.0	|
+| 92	| OSD_TEMP_SENSOR_4_TEMPERATURE					| 2.1.0	|
+| 93	| OSD_TEMP_SENSOR_5_TEMPERATURE					| 2.1.0	|
+| 94	| OSD_TEMP_SENSOR_6_TEMPERATURE					| 2.1.0	|
+| 95	| OSD_TEMP_SENSOR_7_TEMPERATURE					| 2.1.0	|
+| 96	| OSD_ALTITUDE_MSL								| 2.1.0	|
+| 97	| OSD_PLUS_CODE									| 2.1.0	|
+| 98	| OSD_MAP_SCALE									| 2.2.0	|
+| 99	| OSD_MAP_REFERENCE								| 2.2.0	|
+| 100	| OSD_GFORCE									| 2.2.0	|
+| 101	| OSD_GFORCE_X									| 2.2.0	|
+| 102	| OSD_GFORCE_Y									| 2.2.0	|
+| 103	| OSD_GFORCE_Z									| 2.2.0	|
+| 104	| OSD_RC_SOURCE									| 2.2.0	|
+| 105	| OSD_VTX_POWER									| 2.2.0	|
+| 106	| OSD_ESC_RPM									| 2.3.0	|
+| 107	| OSD_ESC_TEMPERATURE							| 2.5.0	|
+| 108	| OSD_AZIMUTH									| 2.6.0	|
+| 109	| OSD_CRSF_RSSI_DBM								| 2.6.0	|
+| 110	| OSD_CRSF_LQ									| 2.6.0	|
+| 111	| OSD_CRSF_SNR_DB								| 2.6.0	|
+| 112	| OSD_CRSF_TX_POWER								| 2.6.0	|
+| 113	| OSD_GVAR_0									| 2.6.0	|
+| 114	| OSD_GVAR_1									| 2.6.0	|
+| 115	| OSD_GVAR_2									| 2.6.0	|
+| 116	| OSD_GVAR_3									| 2.6.0	|
+| 117	| OSD_TPA										| 2.6.0	|
+| 118	| OSD_NAV_FW_CONTROL_SMOOTHNESS					| 2.6.0	|
+| 119	| OSD_VERSION									| 3.0.0	|
+| 120	| OSD_RANGEFINDER								| 3.0.0	|
+| 121	| OSD_PLIMIT_REMAINING_BURST_TIME				| 3.0.0	|
+| 122	| OSD_PLIMIT_ACTIVE_CURRENT_LIMIT				| 3.0.0	|
+| 123	| OSD_PLIMIT_ACTIVE_POWER_LIMIT					| 3.0.0	|
+| 124	| OSD_GLIDESLOPE								| 3.0.1	|
+| 125	| OSD_GPS_MAX_SPEED								| 4.0.0	|
+| 126	| OSD_3D_MAX_SPEED								| 4.0.0	|
+| 127	| OSD_AIR_MAX_SPEED								| 4.0.0	|
+| 128	| OSD_ACTIVE_PROFILE							| 4.0.0	|
+| 129	| OSD_MISSION									| 4.0.0	|
+| 130	| OSD_SWITCH_INDICATOR_0						| 5.0.0	|
+| 131	| OSD_SWITCH_INDICATOR_1						| 5.0.0	|
+| 132	| OSD_SWITCH_INDICATOR_2						| 5.0.0	|
+| 133	| OSD_SWITCH_INDICATOR_3						| 5.0.0	|
+| 134	| OSD_TPA_TIME_CONSTANT							| 5.0.0	|
+| 135	| OSD_FW_LEVEL_TRIM								| 5.0.0	|
+| 136	| OSD_GLIDE_TIME_REMAINING						| 6.0.0	|
+| 137	| OSD_GLIDE_RANGE								| 6.0.0	|
+| 138	| OSD_CLIMB_EFFICIENCY							| 6.0.0	|
+| 139	| OSD_NAV_WP_MULTI_MISSION_INDEX				| 6.0.0	|
+| 140	| OSD_GROUND_COURSE								| 6.0.0	|
+| 141	| OSD_CROSS_TRACK_ERROR							| 6.0.0	|
+| 142	| OSD_PILOT_NAME								| 6.0.0	|
+| 143	| OSD_PAN_SERVO_CENTRED							| 6.0.0	|
--- a/docs/SITL/SITL.md
+++ b/docs/SITL/SITL.md
@ -12,6 +12,7 @@ The sensors are replaced by data provided by a simulator.
 Currently supported are
 - RealFlight  https://www.realflight.com/
 - X-Plane https://www.x-plane.com/
+- fl2sim [replay Blackbox Log via SITL](https://github.com/stronnag/bbl2kml/wiki/fl2sitl), uses the X-Plane protocol.

 INAV SITL communicates for sensor data and control directly with the corresponding simulator, see the documentation of the individual simulators and the Configurator or the command line options.

@ -30,25 +31,38 @@ The following sensors are emulated:
 Select "FAKE" as type for all mentioned, so that they receive the data from the simulator.

 ## Serial ports+
-UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 6761, ...
-By default, UART1 and UART2 are available as MSP connections.
-To connect the Configurator to SITL: Select TCP and connect to ```127.0.0.1:5760``` (if SITL is running on the same machine).
-IPv4 and IPv6 are supported, either raw addresses of hostname lookup.
+UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 5761, ...
+By default, UART1 and UART2 are available as MSP connections. Other UARTs will have TCP listeners if they have an INAV function assigned.
+To connect the Configurator to SITL: Select TCP and connect to ```localhost:5760``` (or ```127.0.0.1:5760``` if your OS doesn't understand `localhost`) (if SITL is running on the same machine).
+IPv4 and IPv6 are supported, either raw addresses or host-name lookup.

 The assignment and status of user UART/TCP connections is displayed on the console.

-![STL-Output](assets/SITL-UART-TCP-Connecion.png)
+```
+INAV 6.1.0 SITL
+[SYSTEM] Init...
+[SIM] No interface specified. Configurator only.
+[EEPROM] Loaded 'eeprom.bin' (32768 of 32768 bytes)
+[SOCKET] Bind TCP :: port 5760 to UART1
+[SOCKET] Bind TCP :: port 5761 to UART2
+[SOCKET] ::1 connected to UART1
+```

 All other interfaces (I2C, SPI, etc.) are not emulated.

 ## Remote control
-Joystick (via simulator) or serial receiver via USB/Serial interface are supported.
+MSP_RX (TCP/IP) or joystick (via simulator) or serial receiver via USB/Serial interface are supported.
+
+### MSP_RX
+
+MSP_RX is the default, 18 channels are supported over TCP/IP serial emulation.

 ### Joystick interface
 Only 8 channels are supported.
 Select "SIM (SITL)" as the receiver and set up a joystick in the simulator, details of which can be found in the documentation for the individual simulators.

 ### Serial Receiver via USB
+
 Connect a serial receiver (e.g. SBUS) to the PC via a UART/USB adapter. Configure the receiver in the Configurator as usual.

 The Configurator offers a built-in option for forwarding the serial data to the SITL TCP port, if SITL is started manually the following option can be used:
@ -65,7 +79,11 @@ For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/)
 For SBUS, the command line arguments of the python script are:
 ```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000```

-Note: Telemetry via return channel through the receiver is not supported by SITL (yet).
+### Telemtry
+
+LTM and MAVLink telemetry are supported, either as a discrete function or shared with MSP.
+
+RX Telemetry via a return channel through the receiver is not yet supported by SITL.

 ## OSD
 For the OSD the program INAV-Sim-OSD is available: https://github.com/Scavanger/INAV-SIM-OSD.
@ -74,20 +92,22 @@ For this, activate MSP-Displayport on a UART/TCP port and connect to the corresp
 Note: INAV-Sim-OSD only works if the simulator is in window mode.

 ## Command line
-The command line options are only necessary if the SITL executable is started by hand, e.g. when debugging.
-For normal use, please use the SITL tab in the configurator.
+
+The command line options are only necessary if the SITL executable is started by hand.
+
+There is also a SITL tab in the  INAV Configurator (6.1.0 and later).

 The following SITL specific command line options are available:

 If SITL is started without command line options, only a serial MSP / CLI connection can be used (e.g. Configurator or other application) can be used.

-```--path``` Full path and file name to config file, if not present, eeprom.bin in the current directory is used. Example: ```C:\INAV_SITL\flying-wing.bin```
+```--path``` Path and file name to config file. If not present, eeprom.bin in the current directory is used. Example: ```C:\INAV_SITL\flying-wing.bin```, ```/home/user/sitl-eeproms/test-eeprom.bin```.

 ```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```

-```--simip=[ip]``` IP address of the simulator, if you specify a simulator with "--sim" and omit this option localhost (127.0.0.1) will be used. Example: ```--simip=172.65.21.15```
+```--simip=[ip]``` Hostname or IP address of the simulator, if you specify a simulator with "--sim" and omit this option IPv4 localhost (`127.0.0.1`) will be used. Example: ```--simip=172.65.21.15```, ```--simip acme-sims.org```, ```--sim ::1```.

-```--simport=[port]``` Port number of the simulator, not necessary for all simulators. Example: ```--simport=4900```
+```--simport=[port]``` Port number of the simulator, not necessary for all simulators. Example: ```--simport=4900```. For the X-Plane protocol, the default port is `49000`.

 ```--useimu``` Use IMU sensor data from the simulator instead of using attitude data directly from the simulator. Not recommended, use only for debugging.

@ -100,6 +120,8 @@ Please also read the documentation of the individual simulators.

 ```--help``` Displays help for the command line options.

+For options that take an argument, either form `--flag=value` or `--flag value` may be used.
+
 ## Running SITL
 It is recommended to start the tools in the following order:
 1. Simulator, aircraft should be ready for take-off
@ -124,6 +146,8 @@ make
 Compile under cygwin, then as in Linux.
 Copy cygwin1.dll into the directory, or include cygwin's /bin/ directory in the environment variable PATH.

+If the build fails (segfault, possibly out of memory), adding `-DCMAKE_BUILD_TYPE=MinRelSize` to the `cmake` command may help.
+
 #### Build manager

 `ninja` may also be used (parallel builds without `-j $(nproc)`):
@ -135,12 +159,12 @@ ninja

 ### Compiler requirements

-* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work.
+* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work. GCC 10 to GCC 13 are known to work.
 * Unix sockets networking. Cygwin is required on Windows (vice `winsock`).
 * Pthreads

 ## Supported environments

-* Linux on x86_64, Aarch64 (e.g. Rpi4), RISC-V (e.g. VisionFive2)
+* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi4), RISCV64 (e.g. VisionFive2)
 * Windows on x86_64
 * FreeBSD (x86_64 at least).
--- a/docs/Settings.md
+++ b/docs/Settings.md
@ -1372,6 +1372,16 @@ Automatic configuration of GPS baudrate(The specified baudrate in configured in

 ---

+### gps_auto_baud_max_supported
+
+Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 230400 |  |  |
+
+---
+
 ### gps_auto_config

 Enable automatic configuration of UBlox GPS receivers.
@ -1422,6 +1432,26 @@ Which SBAS mode to be used

 ---

+### gps_ublox_nav_hz
+
+Navigation update rate for UBLOX7 receivers. Some receivers may limit the maximum number of satellites tracked when set to a higher rate or even stop sending navigation updates if the value is too high. Some M10 devices can do up to 25Hz. 10 is a safe value for M8 and newer.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 5 | 200 |
+
+---
+
+### gps_ublox_use_beidou
+
+Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### gps_ublox_use_galileo

 Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON].
@ -1432,6 +1462,16 @@ Enable use of Galileo satellites. This is at the expense of other regional const

 ---

+### gps_ublox_use_glonass
+
+Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### ground_test_mode

 For developer ground test use. Disables motors, sets heading status = Trusted on FW.
@ -3182,6 +3222,16 @@ Defines at what altitude the descent velocity should start to be `nav_land_minal

 ---

+### nav_landing_bump_detection
+
+Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### nav_manual_climb_rate

 Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]
@ -3232,6 +3282,16 @@ Max allowed above the ground altitude for terrain following mode

 ---

+### nav_mc_althold_throttle
+
+If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| STICK |  |  |
+
+---
+
 ### nav_mc_bank_angle

 Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude
@ -3702,16 +3762,6 @@ Enables or Disables the use of the heading PID controller on fixed wing. Heading

 ---

-### nav_use_midthr_for_althold
-
-If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude
-
-| Default | Min | Max |
-| --- | --- | --- |
-| OFF | OFF | ON |
-
---
-
 ### nav_user_control_mode

 Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction.
--- a/docs/assets/images/StickPositions.png
+++ b/docs/assets/images/StickPositions.png
--- a/docs/assets/images/StickPositions.svg
+++ b/docs/assets/images/StickPositions.svg
--- a/docs/assets/images/StickPositions_trans.png
+++ b/docs/assets/images/StickPositions_trans.png
--- a/docs/boards/SPEEDYBEEF405V3.md
+++ b/docs/boards/SPEEDYBEEF405V3.md
@ -1,3 +1,18 @@
 # SpeedyBee F405 V3

 > Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead
+
+> Notice: The analog OSD and the SD card (blackbox) share the same SPI bus, which can cause problems when trying to use analog OSD and blackbox at the same time.
+
+## Flashing firmware
+
+We often see reports of users having problems flashing new firmware with this baord. The following sugestions usually seem to fix the issues.
+
+* Remove SD Card
+* Disconnect devices from UART1 and UART3
+
+Try removing the SD Card first, and if that still does not fix the issue, disconnect the devices connected to UART1 and UART3.
+
+
+
+
--- a/docs/development/How
+++ b/docs/development/How
@ -0,0 +1,53 @@
+# Introduction
+While many of the instructions here are somewhat generic and will likely work for other projects, the goal of these instructions is to assist a non-developer INAV user to acquire firmware that includes a pull request so he can flash it on his supported fc.
+Building the pull request manually or using custom/unofficial targets is not the focus of this document.
+
+# Why should you test a pull request?
+- You want to volunteer time and resources helping improving INAV for everyone by catching issues before they are introduced in the next release of INAV!
+- You reported or are affected by a bug that has been addressed by this pull request and want to help test the fix
+- You are interested in testing a new feature implemented by this pull request
+
+# Why should you not test a pull request?
+- Pull requests are beta code and may have bugs; bugs may cause you to crash and damage your model
+- Upgrading from the stable version of INAV may require changes to your config that are not yet fully documented
+
+# Before you proceed
+- Read the comments on the pull request you want to test. It may provide useful context and information on known issues, required configuration changes or what branch of the inav-configurator is required.
+- Make sure the pull request has passed all checks, otherwise you may not have pre-compiled firmware images.
+- Make a diff all backup of your existing INAV configuration.
+- Take notes of what INAV target you are using.
+- You will need a recent version of INAV Configurator from master, or even a specific branch. If you don't need a specific branch, [inav-configurator-next](http://seyrsnys.myzen.co.uk/inav-configurator-next/) usually has recent unofficial pre-built versions of INAV Configurator. If your pull requests refers to an inav-configruator pull request, you are likely to need a specific branch of the configurator. In that case you can try to build it from source by following the build [``Instructions``](https://github.com/iNavFlight/inav-configurator#building-and-running-inav-configurator-locally-for-development) or follow instructions on how to do any needed configuration changes using the CLI.
+
+# Finding the pull request
+This is easy, but you will need to be logged in to your GitHub account.
+
+Navigate to the INAV github project and click on [``Pull Requests``](https://github.com/iNavFlight/inav/pulls).
+
+You can just scroll through the list to find a pull request you are interested in, or use the filter bar by typing the name of the pull request, or the number, if you know it.
+
+![Search results](assets/pr_testing/pr_search_result.png)
+
+Once you find the one you are looking for, go ahead an open it!
+
+Click on the ``Checks`` tab
+
+Click on ``Build firmware``, it should take you to the ``Actions`` tab.
+![Search results](assets/pr_testing/build_firmware.png)
+
+You should see a summary with a column saying ``Artifacts`` and a number. Click on the number to be taken to the list of artifacts.
+
+![Search results](assets/pr_testing/actions_summary.png)
+
+On the ``Artifacts`` list, there should be an artifact without SITL in its name.
+
+![Search results](assets/pr_testing/artifact_listing.png)
+
+ Click on it to download the zip file containing pre-compiled firmware images for all INAV official targets. Extract all files and select the firmware for your target using the configurator by clicking on ``Load Firmware [Local]`` button. Don't forget to use the ``Full chip erase`` option, as there are no guarantees the firmware will be compatible with your existing settings.
+
+# I have flashed the new firmware, what should I do next?
+
+- You should configure your model, either manually from scratch, or by loading your diff file. Keep in mind that loading a diff file may not always work, as there may have been some other changes in INAV that require attention. But even if you start from scratch, there are usually many sections that are safe to copy over from your diff.
+- Try to reproduce the bug reported or play around with the new feature. 
+- Once you are done testing, don't forget to report your results on the pull request. Both positive results and issues are valid and welcome feedback.
+
+
--- a/docs/development/assets/pr_testing/actions_summary.png
+++ b/docs/development/assets/pr_testing/actions_summary.png
--- a/docs/development/assets/pr_testing/artifact_listing.png
+++ b/docs/development/assets/pr_testing/artifact_listing.png
--- a/docs/development/assets/pr_testing/build_firmware.png
+++ b/docs/development/assets/pr_testing/build_firmware.png
--- a/docs/development/assets/pr_testing/pr_search_result.png
+++ b/docs/development/assets/pr_testing/pr_search_result.png
--- a/lib/main/MAVLink/mavlink_helpers.h
+++ b/lib/main/MAVLink/mavlink_helpers.h
@ -574,8 +574,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
                                                 mavlink_message_t* r_message, 
                                                 mavlink_status_t* r_mavlink_status)
 {
-	int bufferIndex = 0;
-
 	status->msg_received = MAVLINK_FRAMING_INCOMPLETE;

 	switch (status->parse_state)
@ -801,7 +799,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
 		break;
 	}

-	bufferIndex++;
 	// If a message has been sucessfully decoded, check index
 	if (status->msg_received == MAVLINK_FRAMING_OK)
 	{
--- a/readme.md
+++ b/readme.md
@ -2,10 +2,6 @@

 ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)

-# INAV 6 Horizon Hawk feature freeze
-
-> INAV 6 feature freeze will happen on 29th of January 2023. No new features for INAV 6 will be accepted after that date. 
-
 # INAV Community

 * [INAV Discord Server](https://discord.gg/peg2hhbYwN)
@ -18,13 +14,13 @@
 * On Screen Display (OSD) - both character and pixel style
 * DJI OSD integration: all elements, system messages and warnings
 * Outstanding performance out of the box
-* Position Hold, Altitude Hold, Return To Home and Missions
+* Position Hold, Altitude Hold, Return To Home and Waypoint Missions
 * Excellent support for fixed wing UAVs: airplanes, flying wings
 * Fully configurable mixer that allows to run any hardware you want: multirotor, fixed wing, rovers, boats and other experimental devices
 * Multiple sensor support: GPS, Pitot tube, sonar, lidar, temperature, ESC with BlHeli_32 telemetry
 * SmartAudio and IRC Tramp VTX support
 * Blackbox flight recorder logging
-* Telemetry: SmartPort, FPort, MAVlink, LTM
+* Telemetry: SmartPort, FPort, MAVlink, LTM, CRSF
 * Multi-color RGB LED Strip support
 * Advanced gyro filtering
 * Logic Conditions, Global Functions and Global Variables: you can program INAV with a GUI
@ -42,9 +38,13 @@ Official tool for INAV can be downloaded [here](https://github.com/iNavFlight/in

 Tool for Blackbox logs analysis is available [here](https://github.com/iNavFlight/blackbox-log-viewer/releases)

-### Telemetry screen for OpenTX
+### INAV Blackbox Tools

-Users of OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget)
+Command line tools (`blackbox_decode`, `blackbox_render`) for Blackbox log conversion and analysis [here](https://github.com/iNavFlight/blackbox-tools).
+
+### Telemetry screen for EdgeTX and OpenTX
+
+Users of EdgeTX and OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget)

 ### INAV magnetometer alignment helper

@ -81,7 +81,7 @@ Contributions are welcome and encouraged.  You can contribute in many ways:
 * Telling us your ideas and suggestions.
 * Buying your hardware from this [link](https://inavflight.com/shop/u/bg/)

-A good place to start is Telegram channel or Facebook group. Drop in, say hi.
+A good place to start is the Discord channel, Telegram channel or Facebook group. Drop in, say hi.

 Github issue tracker is a good place to search for existing issues or report a new bug/feature request:

--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@ -501,6 +501,7 @@ main_sources(COMMON_SRC
    io/gps.c
    io/gps.h
    io/gps_ublox.c
+    io/gps_ublox_utils.c
    io/gps_nmea.c
    io/gps_msp.c
    io/gps_fake.c
@ -533,6 +534,8 @@ main_sources(COMMON_SRC
    io/vtx_ffpv24g.h
    io/vtx_control.c
    io/vtx_control.h
+    io/vtx_msp.c
+    io/vtx_msp.h

    navigation/navigation.c
    navigation/navigation.h
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@ -366,6 +366,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
    {"navTgtPos",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
    {"navTgtPos",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
    {"navTgtPos",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
+    {"navTgtHdg", -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
    {"navSurf",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
    {"navAcc",     0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
    {"navAcc",     1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
@ -517,7 +518,7 @@ typedef struct blackboxMainState_s {
    int16_t navTargetVel[XYZ_AXIS_COUNT];
    int32_t navTargetPos[XYZ_AXIS_COUNT];
    int16_t navHeading;
-    int16_t navTargetHeading;
+    uint16_t navTargetHeading;
    int16_t navSurface;
 } blackboxMainState_t;

@ -970,6 +971,7 @@ static void writeIntraframe(void)
            blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
        }

+        blackboxWriteSignedVB(blackboxCurrent->navTargetHeading);
        blackboxWriteSignedVB(blackboxCurrent->navSurface);
    }

@ -1226,6 +1228,7 @@ static void writeInterframe(void)
            blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]);
        }

+        blackboxWriteSignedVB(blackboxCurrent->navTargetHeading - blackboxLast->navTargetHeading);
        blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
    }

@ -1298,6 +1301,16 @@ static void writeSlowFrame(void)
 static void loadSlowState(blackboxSlowState_t *slow)
 {
    memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
+    // Also log Nav auto selected flight modes rather than just those selected by boxmode
+    if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) {
+        slow->flightModeFlags |= (1 << BOXANGLE);
+    }
+    if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
+        slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
+    }
+    if (navigationRequiresTurnAssistance()) {
+        slow->flightModeFlags |= (1 << BOXTURNASSIST);
+    }
    slow->stateFlags = stateFlags;
    slow->failsafePhase = failsafePhase();
    slow->rxSignalReceived = rxIsReceivingSignal();
@ -1326,12 +1339,19 @@ static void loadSlowState(blackboxSlowState_t *slow)
 #endif

    bool valid_temp;
-    valid_temp = getIMUTemperature(&slow->imuTemperature);
-    if (!valid_temp) slow->imuTemperature = TEMPERATURE_INVALID_VALUE;
+    int16_t newTemp = 0;
+    valid_temp = getIMUTemperature(&newTemp);
+    if (valid_temp)
+        slow->imuTemperature = newTemp;
+    else
+        slow->imuTemperature = TEMPERATURE_INVALID_VALUE;

 #ifdef USE_BARO
-    valid_temp = getBaroTemperature(&slow->baroTemperature);
-    if (!valid_temp) slow->baroTemperature = TEMPERATURE_INVALID_VALUE;
+    valid_temp = getBaroTemperature(&newTemp);
+    if (valid_temp)
+        slow->baroTemperature = newTemp;
+    else
+        slow->baroTemperature = TEMPERATURE_INVALID_VALUE;
 #endif

 #ifdef USE_TEMPERATURE_SENSOR
@ -1654,6 +1674,7 @@ static void loadMainState(timeUs_t currentTimeUs)
        blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i];
        blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
    }
+    blackboxCurrent->navTargetHeading = navDesiredHeading;
    blackboxCurrent->navSurface = navActualSurface;
 }

--- a/src/main/cms/cms_menu_navigation.c
+++ b/src/main/cms/cms_menu_navigation.c
@ -48,7 +48,7 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
    OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE),
    OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),
    OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE),
-    OSD_SETTING_ENTRY("MID THR FOR AH", SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD),
+    OSD_SETTING_ENTRY("MC ALTHOLD THROT", SETTING_NAV_MC_ALTHOLD_THROTTLE),
    OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR),
    OSD_SETTING_ENTRY("LANDING DISARM", SETTING_NAV_DISARM_ON_LANDING),

--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@ -190,8 +190,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
    OSD_ELEMENT_ENTRY("TIME (HOUR)", OSD_RTC_TIME),
    OSD_ELEMENT_ENTRY("FLY MODE", OSD_FLYMODE),
    OSD_ELEMENT_ENTRY("NAME", OSD_CRAFT_NAME),
-    OSD_ELEMENT_ENTRY("THR. (MANU)", OSD_THROTTLE_POS),
-    OSD_ELEMENT_ENTRY("THR. (MANU/AUTO)", OSD_THROTTLE_POS_AUTO_THR),
+    OSD_ELEMENT_ENTRY("THR. ", OSD_THROTTLE_POS),
+    OSD_ELEMENT_ENTRY("THR. (SCALED)", OSD_SCALED_THROTTLE_POS),
    OSD_ELEMENT_ENTRY("SYS MESSAGES", OSD_MESSAGES),
    OSD_ELEMENT_ENTRY("VTX CHAN", OSD_VTX_CHANNEL),
    OSD_ELEMENT_ENTRY("CURRENT (A)", OSD_CURRENT_DRAW),
@ -330,13 +330,6 @@ static const OSD_Entry menuOsdElemsEntries[] =
    OSD_BACK_AND_END_ENTRY,
 };

-#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) && defined(USE_TEMPERATURE_SENSOR) && defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
-// All CMS OSD elements should be enabled in this case. The menu has 2 extra
-// elements (label, back+end), but there's an OSD element that we intentionally
-// don't show here (OSD_DEBUG).
-_Static_assert(ARRAYLEN(menuOsdElemsEntries) - 2 + 1 == OSD_ITEM_COUNT, "missing OSD elements in CMS");
-#endif
-
 const CMS_Menu menuOsdElements = {
 #ifdef CMS_MENU_DEBUG
    .GUARD_text = "MENUOSDE",
--- a/src/main/cms/cms_types.h
+++ b/src/main/cms/cms_types.h
@ -192,7 +192,7 @@ typedef struct OSD_SETTING_s {
    const uint8_t step;
 } __attribute__((packed)) OSD_SETTING_t;

-#define OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, type)  { name, NULL, &(const OSD_SETTING_t){ setting, step }, OME_Setting, type }
+#define OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, type)  { name, { NULL }, &(const OSD_SETTING_t){ setting, step }, OME_Setting, type }
 #define OSD_SETTING_ENTRY_TYPE(name, setting, type)             OSD_SETTING_ENTRY_STEP_TYPE(name, setting, 0, type)
 #define OSD_SETTING_ENTRY_STEP(name, setting, step)             OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, 0)
 #define OSD_SETTING_ENTRY(name, setting)                        OSD_SETTING_ENTRY_STEP(name, setting, 0)
--- a/src/main/common/constants.h
+++ b/src/main/common/constants.h
@ -18,9 +18,9 @@
 #pragma once

 #define FEET_PER_MILE                           5280
-#define FEET_PER_NAUTICALMILE                   6076.118
+#define FEET_PER_NAUTICALMILE                   6076.118f
 #define FEET_PER_KILOFEET                       1000 // Used for altitude
 #define METERS_PER_KILOMETER                    1000
-#define METERS_PER_MILE                         1609.344
-#define METERS_PER_FOOT                         3.28084
-#define METERS_PER_NAUTICALMILE                 1852.001
+#define METERS_PER_MILE                         1609.344f
+#define METERS_PER_FOOT                         3.28084f
+#define METERS_PER_NAUTICALMILE                 1852.001f
--- a/src/main/common/encoding.c
+++ b/src/main/common/encoding.c
@ -17,8 +17,6 @@

 #include "platform.h"

-FILE_COMPILE_FOR_SPEED
-
 #include "encoding.h"

 /**
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@ -22,8 +22,6 @@

 #include "platform.h"

-FILE_COMPILE_FOR_SPEED
-
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
@ -318,7 +316,6 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
    filter->y2 = y2;
 }

-FUNCTION_COMPILE_FOR_SIZE
 void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) {
    const float dT = US2S(refreshRate);

@ -335,7 +332,6 @@ void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFr
    }
 }

-FUNCTION_COMPILE_FOR_SIZE
 void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn) {
    *applyFn = nullFilterApply;
    if (cutoffFrequency) {
--- a/src/main/common/fp_pid.c
+++ b/src/main/common/fp_pid.c
@ -24,8 +24,6 @@

 #include "platform.h"

-FILE_COMPILE_FOR_SPEED
-
 #include <math.h>
 #include "common/fp_pid.h"

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

-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
@ -382,36 +380,36 @@ void sensorCalibrationResetState(sensorCalibrationState_t * state)
    }
 }

-void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3])
+void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, float sample[3])
 {
-    state->XtX[0][0] += (float)sample[0] * sample[0];
-    state->XtX[0][1] += (float)sample[0] * sample[1];
-    state->XtX[0][2] += (float)sample[0] * sample[2];
-    state->XtX[0][3] += (float)sample[0];
+    state->XtX[0][0] += sample[0] * sample[0];
+    state->XtX[0][1] += sample[0] * sample[1];
+    state->XtX[0][2] += sample[0] * sample[2];
+    state->XtX[0][3] += sample[0];

-    state->XtX[1][0] += (float)sample[1] * sample[0];
-    state->XtX[1][1] += (float)sample[1] * sample[1];
-    state->XtX[1][2] += (float)sample[1] * sample[2];
-    state->XtX[1][3] += (float)sample[1];
+    state->XtX[1][0] += sample[1] * sample[0];
+    state->XtX[1][1] += sample[1] * sample[1];
+    state->XtX[1][2] += sample[1] * sample[2];
+    state->XtX[1][3] += sample[1];

-    state->XtX[2][0] += (float)sample[2] * sample[0];
-    state->XtX[2][1] += (float)sample[2] * sample[1];
-    state->XtX[2][2] += (float)sample[2] * sample[2];
-    state->XtX[2][3] += (float)sample[2];
+    state->XtX[2][0] += sample[2] * sample[0];
+    state->XtX[2][1] += sample[2] * sample[1];
+    state->XtX[2][2] += sample[2] * sample[2];
+    state->XtX[2][3] += sample[2];

-    state->XtX[3][0] += (float)sample[0];
-    state->XtX[3][1] += (float)sample[1];
-    state->XtX[3][2] += (float)sample[2];
+    state->XtX[3][0] += sample[0];
+    state->XtX[3][1] += sample[1];
+    state->XtX[3][2] += sample[2];
    state->XtX[3][3] += 1;

-    float squareSum = ((float)sample[0] * sample[0]) + ((float)sample[1] * sample[1]) + ((float)sample[2] * sample[2]);
+    float squareSum = (sample[0] * sample[0]) + (sample[1] * sample[1]) + (sample[2] * sample[2]);
    state->XtY[0] += sample[0] * squareSum;
    state->XtY[1] += sample[1] * squareSum;
    state->XtY[2] += sample[2] * squareSum;
    state->XtY[3] += squareSum;
 }

-void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target)
+void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, float sample[3], int target)
 {
    for (int i = 0; i < 3; i++) {
        float scaledSample = (float)sample[i] / (float)target;
@ -523,7 +521,7 @@ float bellCurve(const float x, const float curveWidth)
    return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
 }

-float fast_fsqrtf(const double value) {
+float fast_fsqrtf(const float value) {
    float ret = 0.0f;
 #ifdef USE_ARM_MATH
    arm_sqrt_f32(value, &ret);
@ -540,11 +538,57 @@ float fast_fsqrtf(const double value) {
 // function to calculate the normalization (pythagoras) of a 2-dimensional vector
 float NOINLINE calc_length_pythagorean_2D(const float firstElement, const float secondElement)
 {
-  return fast_fsqrtf(sq(firstElement) + sq(secondElement));
+    return fast_fsqrtf(sq(firstElement) + sq(secondElement));
 }

 // function to calculate the normalization (pythagoras) of a 3-dimensional vector
 float NOINLINE calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement)
 {
-  return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
+    return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
 }
+
+#ifdef SITL_BUILD
+
+/**
+ * @brief Floating-point vector subtraction, equivalent of CMSIS arm_sub_f32.
+*/
+void arm_sub_f32(
+  float * pSrcA,
+  float * pSrcB,
+  float * pDst,
+  uint32_t blockSize)
+{
+    for (uint32_t i = 0; i < blockSize; i++) {
+        pDst[i] = pSrcA[i] - pSrcB[i];
+    }
+}
+
+/**
+ * @brief Floating-point vector scaling, equivalent of CMSIS arm_scale_f32.
+*/
+void arm_scale_f32(
+  float * pSrc,
+  float scale,
+  float * pDst,
+  uint32_t blockSize)
+{
+    for (uint32_t i = 0; i < blockSize; i++) {
+        pDst[i] = pSrc[i] * scale;
+    }
+}
+
+/**
+ * @brief Floating-point vector multiplication, equivalent of CMSIS arm_mult_f32.
+*/
+void arm_mult_f32(
+  float * pSrcA,
+  float * pSrcB,
+  float * pDst,
+  uint32_t blockSize)
+{
+    for (uint32_t i = 0; i < blockSize; i++) {
+        pDst[i] = pSrcA[i] * pSrcB[i];
+    }
+}
+
+#endif
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@ -36,13 +36,13 @@
 #define RAD    (M_PIf / 180.0f)

 #define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100)
-#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100)
+#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100.0f)

-#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10)
+#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f)
 #define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10)

 #define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
-#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)
+#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10.0f)

 #define DEGREES_PER_DEKADEGREE 10
 #define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE)
@ -56,15 +56,15 @@
 #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
 #define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)

-#define CENTIMETERS_TO_CENTIFEET(cm)            (cm / 0.3048)
-#define CENTIMETERS_TO_FEET(cm)                 (cm / 30.48)
-#define CENTIMETERS_TO_METERS(cm)               (cm / 100)
+#define CENTIMETERS_TO_CENTIFEET(cm)            (cm / 0.3048f)
+#define CENTIMETERS_TO_FEET(cm)                 (cm / 30.48f)
+#define CENTIMETERS_TO_METERS(cm)               (cm / 100.0f)

 #define METERS_TO_CENTIMETERS(m)                (m * 100)

-#define CMSEC_TO_CENTIMPH(cms)      (cms * 2.2369363)
-#define CMSEC_TO_CENTIKPH(cms)      (cms * 3.6)
-#define CMSEC_TO_CENTIKNOTS(cms)    (cms * 1.943845)
+#define CMSEC_TO_CENTIMPH(cms)      (cms * 2.2369363f)
+#define CMSEC_TO_CENTIKPH(cms)      (cms * 3.6f)
+#define CMSEC_TO_CENTIKNOTS(cms)    (cms * 1.943845f)

 #define C_TO_KELVIN(temp) (temp + 273.15f)

@ -137,8 +137,8 @@ typedef struct {
 } sensorCalibrationState_t;

 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 sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, float sample[3]);
+void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, float sample[3], int target);
 bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
 bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);

@ -188,6 +188,23 @@ float acos_approx(float x);
 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);

 float bellCurve(const float x, const float curveWidth);
-float fast_fsqrtf(const double value);
+float fast_fsqrtf(const float value);
 float calc_length_pythagorean_2D(const float firstElement, const float secondElement);
 float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement);
+
+/*
+ * The most significat byte is placed at the lowest address
+ * in other words, the most significant byte is "first", on even indexes
+ */
+#define int16_val_big_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx] << 8) | v[2 * idx + 1]))
+/*
+ * The most significat byte is placed at the highest address
+ * in other words, the most significant byte is "last", on odd indexes
+ */
+#define int16_val_little_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx + 1] << 8) | v[2 * idx]))
+
+#ifdef SITL_BUILD
+void arm_sub_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
+void arm_scale_f32(float * pSrc, float scale, float * pDst, uint32_t blockSize);
+void arm_mult_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
+#endif
--- a/src/main/common/memory.c
+++ b/src/main/common/memory.c
@ -52,7 +52,7 @@ void * memAllocate(size_t wantedSize, resourceOwner_e owner)
        retPointer = &dynHeap[dynHeapFreeWord];
        dynHeapFreeWord += wantedWords;
        dynHeapUsage[owner] += wantedWords * sizeof(uint32_t);
-        LOG_DEBUG(SYSTEM, "Memory allocated. Free memory = %d", memGetAvailableBytes());
+        LOG_DEBUG(SYSTEM, "Memory allocated. Free memory = %ld", (unsigned long)memGetAvailableBytes());
    }
    else {
        // OOM
--- a/src/main/common/quaternion.h
+++ b/src/main/common/quaternion.h
@ -58,7 +58,7 @@ static inline void quaternionToAxisAngle(fpAxisAngle_t * result, const fpQuatern
        a.angle -= 2.0f * M_PIf;
    }

-    const float sinVal = sqrt(1.0f - q->q0 * q->q0);
+    const float sinVal = sqrtf(1.0f - q->q0 * q->q0);

    // Axis is only valid when rotation is large enough sin(0.0057 deg) = 0.0001
    if (sinVal > 1e-4f) {
--- a/src/main/common/typeconversion.c
+++ b/src/main/common/typeconversion.c
@ -21,7 +21,6 @@

 #include "maths.h"
 #include "platform.h"
-FILE_COMPILE_FOR_SPEED

 #ifdef REQUIRE_PRINTF_LONG_SUPPORT

--- a/src/main/common/utils.h
+++ b/src/main/common/utils.h
@ -90,10 +90,9 @@ static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; }

 // using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions
 //  than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions)
-#ifdef UNIT_TEST
-// Call memcpy when building unittest - this is easier that asm symbol name mangling (symbols start with _underscore on win32)
+#if defined(UNIT_TEST) || defined(SITL_BUILD)
 #include <string.h>
-static inline void  memcpy_fn(void *destination, const void *source, size_t num) { memcpy(destination, source, num); };
+#define memcpy_fn(destination, source, num) memcpy(destination, source, num)
 #else
 void *memcpy_fn(void *destination, const void *source, size_t num) asm("memcpy");
 #endif
--- a/src/main/drivers/accgyro/accgyro.h
+++ b/src/main/drivers/accgyro/accgyro.h
@ -44,8 +44,8 @@ typedef struct gyroDev_s {
    sensorGyroInterruptStatusFuncPtr intStatusFn;
    sensorGyroUpdateFuncPtr updateFn;
    float scale;                                        // scalefactor
-    int16_t gyroADCRaw[XYZ_AXIS_COUNT];
-    int16_t gyroZero[XYZ_AXIS_COUNT];
+    float gyroADCRaw[XYZ_AXIS_COUNT];
+    float gyroZero[XYZ_AXIS_COUNT];
    uint8_t imuSensorToUse;
    uint8_t lpf;                                        // Configuration value: Hardware LPF setting
    uint32_t requestedSampleIntervalUs;                 // Requested sample interval
@ -59,7 +59,7 @@ typedef struct accDev_s {
    sensorAccInitFuncPtr initFn;                        // initialize function
    sensorAccReadFuncPtr readFn;                        // read 3 axis data function
    uint16_t acc_1G;
-    int16_t ADCRaw[XYZ_AXIS_COUNT];
+    float ADCRaw[XYZ_AXIS_COUNT];
    uint8_t imuSensorToUse;
    sensor_align_e accAlign;
 } accDev_t;
--- a/src/main/drivers/accgyro/accgyro_bmi088.c
+++ b/src/main/drivers/accgyro/accgyro_bmi088.c
@ -136,9 +136,9 @@ static bool bmi088GyroRead(gyroDev_t *gyro)
    uint8_t gyroRaw[6];

    if (busReadBuf(gyro->busDev, REGG_RATE_X_LSB, gyroRaw, 6)) {
-        gyro->gyroADCRaw[X] = (int16_t)((gyroRaw[1] << 8) | gyroRaw[0]);
-        gyro->gyroADCRaw[Y] = (int16_t)((gyroRaw[3] << 8) | gyroRaw[2]);
-        gyro->gyroADCRaw[Z] = (int16_t)((gyroRaw[5] << 8) | gyroRaw[4]);
+        gyro->gyroADCRaw[X] = (float) int16_val_little_endian(gyroRaw, 0);
+        gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(gyroRaw, 1);
+        gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(gyroRaw, 2);
        return true;
    }

@ -151,9 +151,9 @@ static bool bmi088AccRead(accDev_t *acc)

    if (busReadBuf(acc->busDev, REGA_STATUS, buffer, 2) && (buffer[1] & 0x80) && busReadBuf(acc->busDev, REGA_X_LSB, buffer, 7)) {
        // first byte is discarded, see datasheet
-        acc->ADCRaw[X] = (int32_t)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4);
-        acc->ADCRaw[Y] = (int32_t)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4);
-        acc->ADCRaw[Z] = (int32_t)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4);
+        acc->ADCRaw[X] = (float)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4);
+        acc->ADCRaw[Y] = (float)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4);
+        acc->ADCRaw[Z] = (float)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4);
        return true;
    }

--- a/src/main/drivers/accgyro/accgyro_bmi160.c
+++ b/src/main/drivers/accgyro/accgyro_bmi160.c
@ -180,9 +180,9 @@ bool bmi160GyroReadScratchpad(gyroDev_t *gyro)
    ctx->lastReadStatus = busReadBuf(gyro->busDev, BMI160_REG_GYR_DATA_X_LSB, ctx->gyroRaw, 6 + 6);

    if (ctx->lastReadStatus) {
-        gyro->gyroADCRaw[X] = (int16_t)((ctx->gyroRaw[1] << 8) | ctx->gyroRaw[0]);
-        gyro->gyroADCRaw[Y] = (int16_t)((ctx->gyroRaw[3] << 8) | ctx->gyroRaw[2]);
-        gyro->gyroADCRaw[Z] = (int16_t)((ctx->gyroRaw[5] << 8) | ctx->gyroRaw[4]);
+        gyro->gyroADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0);
+        gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1);
+        gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2);

        return true;
    }
@ -195,9 +195,9 @@ bool bmi160AccReadScratchpad(accDev_t *acc)
    bmi160ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);

    if (ctx->lastReadStatus) {
-        acc->ADCRaw[X] = (int16_t)((ctx->accRaw[1] << 8) | ctx->accRaw[0]);
-        acc->ADCRaw[Y] = (int16_t)((ctx->accRaw[3] << 8) | ctx->accRaw[2]);
-        acc->ADCRaw[Z] = (int16_t)((ctx->accRaw[5] << 8) | ctx->accRaw[4]);
+        acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0);
+        acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1);
+        acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2);
        return true;
    }

--- a/src/main/drivers/accgyro/accgyro_bmi270.c
+++ b/src/main/drivers/accgyro/accgyro_bmi270.c
@ -254,9 +254,9 @@ static bool bmi270yroReadScratchpad(gyroDev_t *gyro)
    ctx->lastReadStatus = busReadBuf(gyro->busDev, BMI270_REG_ACC_DATA_X_LSB, &ctx->__padding_dummy, 6 + 6 + 1);

    if (ctx->lastReadStatus) {
-        gyro->gyroADCRaw[X] = (int16_t)((ctx->gyroRaw[1] << 8) | ctx->gyroRaw[0]);
-        gyro->gyroADCRaw[Y] = (int16_t)((ctx->gyroRaw[3] << 8) | ctx->gyroRaw[2]);
-        gyro->gyroADCRaw[Z] = (int16_t)((ctx->gyroRaw[5] << 8) | ctx->gyroRaw[4]);
+        gyro->gyroADCRaw[X] = (float) int16_val_little_endian(ctx->gyroRaw, 0);
+        gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(ctx->gyroRaw, 1);
+        gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(ctx->gyroRaw, 2);

        return true;
    }
@ -269,9 +269,9 @@ static bool bmi270AccReadScratchpad(accDev_t *acc)
    bmi270ContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);

    if (ctx->lastReadStatus) {
-        acc->ADCRaw[X] = (int16_t)((ctx->accRaw[1] << 8) | ctx->accRaw[0]);
-        acc->ADCRaw[Y] = (int16_t)((ctx->accRaw[3] << 8) | ctx->accRaw[2]);
-        acc->ADCRaw[Z] = (int16_t)((ctx->accRaw[5] << 8) | ctx->accRaw[4]);
+        acc->ADCRaw[X] = (float) int16_val_little_endian(ctx->accRaw, 0);
+        acc->ADCRaw[Y] = (float) int16_val_little_endian(ctx->accRaw, 1);
+        acc->ADCRaw[Z] = (float) int16_val_little_endian(ctx->accRaw, 2);
        return true;
    }

--- a/src/main/drivers/accgyro/accgyro_fake.c
+++ b/src/main/drivers/accgyro/accgyro_fake.c
@ -32,7 +32,7 @@

 #ifdef USE_IMU_FAKE

-static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
+static float fakeGyroADC[XYZ_AXIS_COUNT];

 static void fakeGyroInit(gyroDev_t *gyro)
 {
--- a/src/main/drivers/accgyro/accgyro_icm42605.c
+++ b/src/main/drivers/accgyro/accgyro_icm42605.c
@ -98,9 +98,9 @@ static bool icm42605AccRead(accDev_t *acc)
        return false;
    }

-    acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
-    acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
-    acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
+    acc->ADCRaw[X] = (float) int16_val_big_endian(data, 0);
+    acc->ADCRaw[Y] = (float) int16_val_big_endian(data, 1);
+    acc->ADCRaw[Z] = (float) int16_val_big_endian(data, 2);

    return true;
 }
@ -231,9 +231,9 @@ static bool icm42605GyroRead(gyroDev_t *gyro)
        return false;
    }

-    gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
-    gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
-    gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
+    gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0);
+    gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1);
+    gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2);

    return true;
 }
--- a/src/main/drivers/accgyro/accgyro_lsm6dxx.c
+++ b/src/main/drivers/accgyro/accgyro_lsm6dxx.c
@ -184,9 +184,9 @@ static bool lsm6dxxAccRead(accDev_t *acc)
    if (!ack) {
        return false;
    }
-    acc->ADCRaw[X] = (int16_t)((data[1] << 8) | data[0]);
-    acc->ADCRaw[Y] = (int16_t)((data[3] << 8) | data[2]);
-    acc->ADCRaw[Z] = (int16_t)((data[5] << 8) | data[4]);
+    acc->ADCRaw[X] = (float) int16_val_little_endian(data, 0);
+    acc->ADCRaw[Y] = (float) int16_val_little_endian(data, 1);
+    acc->ADCRaw[Z] = (float) int16_val_little_endian(data, 2);
    return true; 
 }

@ -197,9 +197,9 @@ static bool lsm6dxxGyroRead(gyroDev_t *gyro)
    if (!ack) {
        return false;
    }
-    gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[0]);
-    gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[2]);
-    gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[4]);
+    gyro->gyroADCRaw[X] = (float) int16_val_little_endian(data, 0);
+    gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(data, 1);
+    gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(data, 2);
    return true;
 }

--- a/src/main/drivers/accgyro/accgyro_mpu.c
+++ b/src/main/drivers/accgyro/accgyro_mpu.c
@ -43,8 +43,6 @@
 // Check busDevice scratchpad memory size
 STATIC_ASSERT(sizeof(mpuContextData_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);

-#define int16_val(v, idx) ((int16_t)(((uint8_t)v[2 * idx] << 8) | v[2 * idx + 1]))
-
 static const gyroFilterAndRateConfig_t mpuGyroConfigs[] = {
    { GYRO_LPF_256HZ,   8000,   { MPU_DLPF_256HZ,   0  } },
    { GYRO_LPF_256HZ,   4000,   { MPU_DLPF_256HZ,   1  } },
@ -83,9 +81,9 @@ bool mpuGyroRead(gyroDev_t *gyro)
        return false;
    }

-    gyro->gyroADCRaw[X] = int16_val(data, 0);
-    gyro->gyroADCRaw[Y] = int16_val(data, 1);
-    gyro->gyroADCRaw[Z] = int16_val(data, 2);
+    gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0);
+    gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1);
+    gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2);

    return true;
 }
@ -102,9 +100,9 @@ bool mpuGyroReadScratchpad(gyroDev_t *gyro)
    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(busDev);

    if (mpuUpdateSensorContext(busDev, ctx)) {
-        gyro->gyroADCRaw[X] = int16_val(ctx->gyroRaw, 0);
-        gyro->gyroADCRaw[Y] = int16_val(ctx->gyroRaw, 1);
-        gyro->gyroADCRaw[Z] = int16_val(ctx->gyroRaw, 2);
+        gyro->gyroADCRaw[X] = (float) int16_val_big_endian(ctx->gyroRaw, 0);
+        gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(ctx->gyroRaw, 1);
+        gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(ctx->gyroRaw, 2);
        return true;
    }

@ -116,9 +114,9 @@ bool mpuAccReadScratchpad(accDev_t *acc)
    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);

    if (ctx->lastReadStatus) {
-        acc->ADCRaw[X] = int16_val(ctx->accRaw, 0);
-        acc->ADCRaw[Y] = int16_val(ctx->accRaw, 1);
-        acc->ADCRaw[Z] = int16_val(ctx->accRaw, 2);
+        acc->ADCRaw[X] = (float) int16_val_big_endian(ctx->accRaw, 0);
+        acc->ADCRaw[Y] = (float) int16_val_big_endian(ctx->accRaw, 1);
+        acc->ADCRaw[Z] = (float) int16_val_big_endian(ctx->accRaw, 2);
        return true;
    }

@ -131,7 +129,7 @@ bool mpuTemperatureReadScratchpad(gyroDev_t *gyro, int16_t * data)

    if (ctx->lastReadStatus) {
        // Convert to degC*10: degC = raw / 340 + 36.53
-        *data = int16_val(ctx->tempRaw, 0) / 34 + 365;
+        *data = int16_val_big_endian(ctx->tempRaw, 0) / 34 + 365;
        return true;
    }

--- a/src/main/drivers/adc.c
+++ b/src/main/drivers/adc.c
@ -47,6 +47,13 @@
 #ifndef ADC_CHANNEL_4_INSTANCE
 #define ADC_CHANNEL_4_INSTANCE  ADC_INSTANCE
 #endif
+#ifndef ADC_CHANNEL_5_INSTANCE
+#define ADC_CHANNEL_5_INSTANCE  ADC_INSTANCE
+#endif
+#ifndef ADC_CHANNEL_6_INSTANCE
+#define ADC_CHANNEL_6_INSTANCE  ADC_INSTANCE
+#endif
+

 #ifdef USE_ADC

@ -99,7 +106,7 @@ uint16_t adcGetChannel(uint8_t function)
    }
 }

-#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN)
+#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN) || defined(ADC_CHANNEL_5_PIN) || defined(ADC_CHANNEL_6_PIN)
 static bool isChannelInUse(int channel)
 {
    for (int i = 0; i < ADC_FUNCTION_COUNT; i++) {
@ -111,7 +118,7 @@ static bool isChannelInUse(int channel)
 }
 #endif

-#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN)
+#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN) || !defined(ADC_CHANNEL_5_PIN) || !defined(ADC_CHANNEL_6_PIN)
 static void disableChannelMapping(int channel)
 {
    for (int i = 0; i < ADC_FUNCTION_COUNT; i++) {
@ -192,6 +199,33 @@ void adcInit(drv_adc_config_t *init)
    disableChannelMapping(ADC_CHN_4);
 #endif

+#ifdef ADC_CHANNEL_5_PIN
+    if (isChannelInUse(ADC_CHN_5)) {
+        adcConfig[ADC_CHN_5].adcDevice = adcDeviceByInstance(ADC_CHANNEL_5_INSTANCE);
+        if (adcConfig[ADC_CHN_5].adcDevice != ADCINVALID) {
+            adcConfig[ADC_CHN_5].tag = IO_TAG(ADC_CHANNEL_5_PIN);
+#if defined(USE_ADC_AVERAGING)
+            activeChannelCount[adcConfig[ADC_CHN_5].adcDevice] += 1;
+#endif
+        }
+    }
+#else
+    disableChannelMapping(ADC_CHN_5);
+#endif
+
+#ifdef ADC_CHANNEL_6_PIN
+    if (isChannelInUse(ADC_CHN_6)) {
+        adcConfig[ADC_CHN_6].adcDevice = adcDeviceByInstance(ADC_CHANNEL_6_INSTANCE);
+        if (adcConfig[ADC_CHN_6].adcDevice != ADCINVALID) {
+            adcConfig[ADC_CHN_6].tag = IO_TAG(ADC_CHANNEL_6_PIN);
+#if defined(USE_ADC_AVERAGING)
+            activeChannelCount[adcConfig[ADC_CHN_6].adcDevice] += 1;
+#endif
+        }
+    }
+#else
+    disableChannelMapping(ADC_CHN_6);
+#endif

    adcHardwareInit(init);
 }
--- a/src/main/drivers/adc.h
+++ b/src/main/drivers/adc.h
@ -33,7 +33,9 @@ typedef enum {
    ADC_CHN_2,
    ADC_CHN_3,
    ADC_CHN_4,
-    ADC_CHN_MAX = ADC_CHN_4,
+	ADC_CHN_5,
+	ADC_CHN_6,
+    ADC_CHN_MAX = ADC_CHN_6,
    ADC_CHN_COUNT
 } adcChannel_e;

--- a/src/main/drivers/barometer/barometer_msp.c
+++ b/src/main/drivers/barometer/barometer_msp.c
@ -39,6 +39,10 @@
 #include "drivers/barometer/barometer.h"
 #include "drivers/barometer/barometer_msp.h"

+#include "sensors/sensors.h"
+#include "sensors/barometer.h"
+#include "fc/runtime_config.h"
+
 #include "msp/msp_protocol_v2_sensor_msg.h"

 #define MSP_BARO_TIMEOUT_MS      250     // Less than 4Hz updates is considered a failure
@ -47,6 +51,8 @@ static int32_t mspBaroPressure;
 static int32_t mspBaroTemperature;
 static timeMs_t mspBaroLastUpdateMs;

+static bool mspBaroStarted = false;
+
 static bool mspBaroStartGet(baroDev_t * baro)
 {
    UNUSED(baro);
@ -57,7 +63,9 @@ static bool mspBaroCalculate(baroDev_t * baro, int32_t *pressure, int32_t *tempe
 {
    UNUSED(baro);

-    if ((millis() - mspBaroLastUpdateMs) > MSP_BARO_TIMEOUT_MS) {
+    if (((millis() - mspBaroLastUpdateMs) > MSP_BARO_TIMEOUT_MS)) {
+        sensorsClear(SENSOR_BARO);
+        mspBaroStarted = false;
        return false;
    }

@ -77,12 +85,21 @@ void mspBaroReceiveNewData(uint8_t * bufferPtr)
    mspBaroPressure = pkt->pressurePa;
    mspBaroTemperature = pkt->temp;
    mspBaroLastUpdateMs = millis();
+    
+    // This should only happen after a reset (!ARMING_FLAG(WAS_EVER_ARMED)) to avoid 
+    // getting calibrations mid-air or on a surface that is above the home position
+    if (mspBaroStarted == false && !ARMING_FLAG(WAS_EVER_ARMED)){
+        baroStartCalibration();
+        mspBaroStarted = true;
+        sensorsSet(SENSOR_BARO);
+    }
 }

 bool mspBaroDetect(baroDev_t *baro)
 {
    mspBaroPressure = 101325;    // pressure in Pa (0m MSL)
    mspBaroTemperature = 2500;   // temperature in 0.01 C = 25 deg
+    mspBaroLastUpdateMs = 0;

    // these are dummy as temperature is measured as part of pressure
    baro->ut_delay = 10000;
--- a/src/main/drivers/bus_i2c_hal.c
+++ b/src/main/drivers/bus_i2c_hal.c
@ -85,7 +85,7 @@ static i2cDevice_t i2cHardwareMap[I2CDEV_COUNT] = {
    { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1L(I2C2), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn, .af = GPIO_AF4_I2C2 },
    { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1L(I2C3), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn, .af = GPIO_AF4_I2C3 },
 #if defined(USE_I2C_DEVICE_4)
-    { .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB1L(I2C4), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 }
+    { .dev = I2C4, .scl = IO_TAG(I2C4_SCL), .sda = IO_TAG(I2C4_SDA), .rcc = RCC_APB4(I2C4), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C4_EV_IRQn, .er_irq = I2C4_ER_IRQn, .af = GPIO_AF4_I2C4 }
 #endif
 #endif
 };
--- a/src/main/drivers/bus_quadspi.c
+++ b/src/main/drivers/bus_quadspi.c
@ -0,0 +1,310 @@
+/*
+ * 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/>.
+ *
+ * Author: Dominic Clifton
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef USE_QUADSPI
+
+#include "drivers/bus_quadspi.h"
+#include "drivers/bus_quadspi_impl.h"
+#include "drivers/exti.h"
+#include "drivers/io.h"
+#include "drivers/rcc.h"
+
+static const quadSpiConfig_t quadSpiConfig[] = {
+#ifdef USE_QUADSPI_DEVICE_1
+        { .device = QUADSPIDEV_1, .clk = IO_TAG(QUADSPI1_SCK_PIN),
+          .bk1CS = IO_TAG(QUADSPI1_BK1_CS_PIN),
+          .bk1IO0 = IO_TAG(QUADSPI1_BK1_IO0_PIN), .bk1IO1 = IO_TAG(QUADSPI1_BK1_IO1_PIN), .bk1IO2 = IO_TAG(QUADSPI1_BK1_IO2_PIN), .bk1IO3 = IO_TAG(QUADSPI1_BK1_IO3_PIN),
+          .bk2CS = IO_TAG(QUADSPI1_BK2_CS_PIN),
+          .bk2IO0 = IO_TAG(QUADSPI1_BK2_IO0_PIN), .bk2IO1 = IO_TAG(QUADSPI1_BK2_IO1_PIN), .bk2IO2 = IO_TAG(QUADSPI1_BK2_IO2_PIN), .bk2IO3 = IO_TAG(QUADSPI1_BK2_IO3_PIN),
+          .mode = QUADSPI1_MODE, .csFlags = QUADSPI1_CS_FLAGS
+        }
+#endif
+};
+
+quadSpiDevice_t quadSpiDevice[QUADSPIDEV_COUNT] = { 0 };
+
+QUADSPIDevice quadSpiDeviceByInstance(QUADSPI_TypeDef *instance)
+{
+#ifdef USE_QUADSPI_DEVICE_1
+    if (instance == QUADSPI) {
+        return QUADSPIDEV_1;
+    }
+#endif
+
+    return QUADSPIINVALID;
+}
+
+QUADSPI_TypeDef *quadSpiInstanceByDevice(QUADSPIDevice device)
+{
+    if (device == QUADSPIINVALID || device >= QUADSPIDEV_COUNT) {
+        return NULL;
+    }
+
+    return quadSpiDevice[device].dev;
+}
+
+bool quadSpiInit(QUADSPIDevice device)
+{
+    switch (device) {
+    case QUADSPIINVALID:
+        return false;
+    case QUADSPIDEV_1:
+#ifdef USE_QUADSPI_DEVICE_1
+        quadSpiInitDevice(device);
+        return true;
+#else
+        break;
+#endif
+    }
+    return false;
+}
+
+uint32_t quadSpiTimeoutUserCallback(QUADSPI_TypeDef *instance)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    if (device == QUADSPIINVALID) {
+        return -1;
+    }
+    quadSpiDevice[device].errorCount++;
+    return quadSpiDevice[device].errorCount;
+}
+
+uint16_t quadSpiGetErrorCounter(QUADSPI_TypeDef *instance)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    if (device == QUADSPIINVALID) {
+        return 0;
+    }
+    return quadSpiDevice[device].errorCount;
+}
+
+void quadSpiResetErrorCounter(QUADSPI_TypeDef *instance)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    if (device != QUADSPIINVALID) {
+        quadSpiDevice[device].errorCount = 0;
+    }
+}
+
+const quadSpiHardware_t quadSpiHardware[] = {
+#ifdef STM32H7
+    {
+        .device = QUADSPIDEV_1,
+        .reg = QUADSPI,
+        .clkPins = {
+            { DEFIO_TAG_E(PB2),  GPIO_AF9_QUADSPI },
+        },
+        .bk1IO0Pins = {
+            { DEFIO_TAG_E(PC9),  GPIO_AF9_QUADSPI },
+            { DEFIO_TAG_E(PD11), GPIO_AF9_QUADSPI },
+            { DEFIO_TAG_E(PF8),  GPIO_AF10_QUADSPI },
+        },
+        .bk1IO1Pins = {
+            { DEFIO_TAG_E(PC10), GPIO_AF9_QUADSPI },
+            { DEFIO_TAG_E(PD12), GPIO_AF9_QUADSPI },
+            { DEFIO_TAG_E(PF9),  GPIO_AF10_QUADSPI },
+        },
+        .bk1IO2Pins = {
+            { DEFIO_TAG_E(PE2),  GPIO_AF9_QUADSPI },
+            { DEFIO_TAG_E(PF7),  GPIO_AF9_QUADSPI },
+        },
+        .bk1IO3Pins = {
+            { DEFIO_TAG_E(PA1),  GPIO_AF9_QUADSPI },
+            { DEFIO_TAG_E(PD13), GPIO_AF9_QUADSPI },
+            { DEFIO_TAG_E(PF6),  GPIO_AF9_QUADSPI },
+        },
+        .bk1CSPins = {
+            { DEFIO_TAG_E(PB6),  GPIO_AF10_QUADSPI },
+            { DEFIO_TAG_E(PB10), GPIO_AF9_QUADSPI },
+            { DEFIO_TAG_E(PG6),  GPIO_AF10_QUADSPI },
+        },
+        .bk2IO0Pins = {
+            { DEFIO_TAG_E(PE7),  GPIO_AF10_QUADSPI },
+            //{ DEFIO_TAG_E(PH7),  GPIO_AF9_QUADSPI }, // FIXME regenerate io_def_generated with support for GPIO 'H'
+        },
+        .bk2IO1Pins = {
+            { DEFIO_TAG_E(PE8),  GPIO_AF10_QUADSPI },
+            //{ DEFIO_TAG_E(PH3),  GPIO_AF9_QUADSPI }, // FIXME regenerate io_def_generated with support for GPIO 'H'
+        },
+        .bk2IO2Pins = {
+            { DEFIO_TAG_E(PE9),  GPIO_AF10_QUADSPI },
+            { DEFIO_TAG_E(PG9),  GPIO_AF9_QUADSPI },
+        },
+        .bk2IO3Pins = {
+            { DEFIO_TAG_E(PE10),  GPIO_AF10_QUADSPI },
+            { DEFIO_TAG_E(PG14),  GPIO_AF9_QUADSPI },
+        },
+        .bk2CSPins = {
+            { DEFIO_TAG_E(PC11),  GPIO_AF9_QUADSPI },
+        },
+        .rcc = RCC_AHB3(QSPI),
+    },
+#endif
+};
+
+const quadSpiConfig_t * getQuadSpiConfig(QUADSPIDevice device)
+{
+    const quadSpiConfig_t * pConfig = NULL;
+
+    for (size_t configIndex = 0; configIndex < ARRAYLEN(quadSpiConfig); configIndex++) {
+        if (quadSpiConfig[configIndex].device == device) {
+            pConfig = &quadSpiConfig[configIndex];
+            break;
+        }
+    }
+
+    return pConfig;
+}
+
+
+void quadSpiPinConfigure(QUADSPIDevice device)
+{
+    if (device == QUADSPIINVALID) {
+        return;
+    }
+
+    const quadSpiConfig_t * pConfig = getQuadSpiConfig(device);
+
+    if (pConfig == NULL) {
+        return;
+    }
+
+    for (size_t hwindex = 0; hwindex < ARRAYLEN(quadSpiHardware); hwindex++) {
+        const quadSpiHardware_t *hw = &quadSpiHardware[hwindex];
+
+        if (hw->device != device) {
+            continue;
+        }
+
+        quadSpiDevice_t *pDev = &quadSpiDevice[device];
+
+        for (int pindex = 0; pindex < MAX_QUADSPI_PIN_SEL; pindex++) {
+            if (pConfig->clk == hw->clkPins[pindex].pin) {
+                pDev->clk = hw->clkPins[pindex].pin;
+            }
+            //
+            // BK1
+            //
+            if (pConfig->bk1IO0 == hw->bk1IO0Pins[pindex].pin) {
+                pDev->bk1IO0 = hw->bk1IO0Pins[pindex].pin;
+                pDev->bk1IO0AF = hw->bk1IO0Pins[pindex].af;
+            }
+            if (pConfig->bk1IO1 == hw->bk1IO1Pins[pindex].pin) {
+                pDev->bk1IO1 = hw->bk1IO1Pins[pindex].pin;
+                pDev->bk1IO1AF = hw->bk1IO1Pins[pindex].af;
+            }
+            if (pConfig->bk1IO2 == hw->bk1IO2Pins[pindex].pin) {
+                pDev->bk1IO2 = hw->bk1IO2Pins[pindex].pin;
+                pDev->bk1IO2AF = hw->bk1IO2Pins[pindex].af;
+            }
+            if (pConfig->bk1IO3 == hw->bk1IO3Pins[pindex].pin) {
+                pDev->bk1IO3 = hw->bk1IO3Pins[pindex].pin;
+                pDev->bk1IO3AF = hw->bk1IO3Pins[pindex].af;
+            }
+            if (pConfig->bk1CS == hw->bk1CSPins[pindex].pin) {
+                pDev->bk1CS = hw->bk1CSPins[pindex].pin;
+                pDev->bk1CSAF = hw->bk1CSPins[pindex].af;
+            }
+            //
+            // BK2
+            //
+            if (pConfig->bk2IO0 == hw->bk2IO0Pins[pindex].pin) {
+                pDev->bk2IO0 = hw->bk2IO0Pins[pindex].pin;
+                pDev->bk2IO0AF = hw->bk2IO0Pins[pindex].af;
+            }
+            if (pConfig->bk2IO1 == hw->bk2IO1Pins[pindex].pin) {
+                pDev->bk2IO1 = hw->bk2IO1Pins[pindex].pin;
+                pDev->bk2IO1AF = hw->bk2IO1Pins[pindex].af;
+            }
+            if (pConfig->bk2IO1 == hw->bk2IO2Pins[pindex].pin) {
+                pDev->bk2IO2 = hw->bk2IO2Pins[pindex].pin;
+                pDev->bk2IO2AF = hw->bk2IO2Pins[pindex].af;
+            }
+            if (pConfig->bk2IO2 == hw->bk2IO3Pins[pindex].pin) {
+                pDev->bk2IO3 = hw->bk2IO3Pins[pindex].pin;
+                pDev->bk2IO3AF = hw->bk2IO3Pins[pindex].af;
+            }
+            if (pConfig->bk2IO3 == hw->bk2CSPins[pindex].pin) {
+                pDev->bk2CS = hw->bk2CSPins[pindex].pin;
+                pDev->bk2CSAF = hw->bk2CSPins[pindex].af;
+            }
+        }
+
+        if ((pConfig->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
+            pDev->bk1CS = pConfig->bk1CS;
+        }
+        if ((pConfig->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
+            pDev->bk2CS = pConfig->bk2CS;
+        }
+
+        bool haveResources = true;
+
+        // clock pins
+
+        haveResources = haveResources && pDev->clk;
+
+        // data pins
+
+        bool needBK1 = (pConfig->mode == QUADSPI_MODE_DUAL_FLASH) || (pConfig->mode == QUADSPI_MODE_BK1_ONLY);
+        if (needBK1) {
+            bool haveBK1Resources = pDev->bk1IO0 && pDev->bk1IO1 && pDev->bk1IO2 && pDev->bk1IO3 && pDev->bk1CS;
+            haveResources = haveResources && haveBK1Resources;
+        }
+
+        bool needBK2 = (pConfig->mode == QUADSPI_MODE_DUAL_FLASH) || (pConfig->mode == QUADSPI_MODE_BK2_ONLY);
+        if (needBK2) {
+            bool haveBK2Resources = pDev->bk2IO0 && pDev->bk2IO1 && pDev->bk2IO2 && pDev->bk2IO3;
+            haveResources = haveResources && haveBK2Resources;
+        }
+
+        // cs pins
+
+        if (needBK1) {
+            haveResources = haveResources && pDev->bk1CS;
+        }
+
+        bool needBK2CS =
+            (pConfig->mode == QUADSPI_MODE_DUAL_FLASH && (pConfig->csFlags & QUADSPI_CS_MODE_MASK) == QUADSPI_CS_MODE_SEPARATE) ||
+            (pConfig->mode == QUADSPI_MODE_BK2_ONLY);
+
+        if (needBK2CS) {
+            haveResources = haveResources && pDev->bk2CS;
+        }
+
+        if (haveResources) {
+            pDev->dev = hw->reg;
+            pDev->rcc = hw->rcc;
+        }
+
+        // copy mode and flags
+        pDev->mode = pConfig->mode;
+        pDev->csFlags = pConfig->csFlags;
+    }
+}
+
+#endif
--- a/src/main/drivers/bus_quadspi.h
+++ b/src/main/drivers/bus_quadspi.h
@ -0,0 +1,158 @@
+/*
+ * 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/>.
+ *
+ * Author: Dominic Clifton
+ */
+
+#pragma once
+
+#include "drivers/io_types.h"
+#include "drivers/rcc_types.h"
+
+/*
+ * Quad SPI supports 1/2/4 wire modes
+ *
+ * 1LINE is like SPI MISO/MOSI using D0 (MOSI)/D1(MISO).
+ * 2LINE uses D0, D1 (bidirectional).
+ * 4LINE uses D0..D3 (bidirectional)
+ *
+ * See ST Micros' AN4760 "Quad-SPI (QSPI) interface on STM32 microcontrollers"
+ */
+
+#ifdef USE_QUADSPI
+
+#if !defined(STM32H7)
+#error Quad SPI unsupported on this MCU
+#endif
+
+#define QUADSPI_IO_AF_BK_IO_CFG           IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
+#define QUADSPI_IO_AF_CLK_CFG             IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL)
+#define QUADSPI_IO_AF_BK_CS_CFG           IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
+#define QUADSPI_IO_BK_CS_CFG              IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_HIGH, GPIO_PULLUP)
+
+typedef enum {
+    /* QSPI freq = CLK /(1 + ClockPrescaler) = 200 MHz/(1+x) */
+    QUADSPI_CLOCK_INITIALISATION = 255, //  0.78125 Mhz
+    QUADSPI_CLOCK_SLOW           = 19,  // 10.00000 Mhz
+    QUADSPI_CLOCK_STANDARD       = 9,   // 20.00000 MHz
+    QUADSPI_CLOCK_FAST           = 3,   // 50.00000 MHz
+    QUADSPI_CLOCK_ULTRAFAST      = 1    //100.00000 MHz
+} QUADSPIClockDivider_e;
+
+typedef enum QUADSPIDevice {
+    QUADSPIINVALID = -1,
+    QUADSPIDEV_1   = 0,
+} QUADSPIDevice;
+
+#define QUADSPIDEV_COUNT 1
+
+// Macros to convert between CLI bus number and SPIDevice.
+#define QUADSPI_CFG_TO_DEV(x)   ((x) - 1)
+#define QUADSPI_DEV_TO_CFG(x)   ((x) + 1)
+
+typedef enum {
+    QUADSPI_MODE_BK1_ONLY = 0,
+    QUADSPI_MODE_BK2_ONLY,
+    QUADSPI_MODE_DUAL_FLASH,
+} quadSpiMode_e;
+
+//
+// QUADSPI1_CS_FLAGS
+//
+#define QUADSPI_BK1_CS_MASK         ((1 << 1) | (1 << 0))
+
+#define QUADSPI_BK1_CS_NONE         ((0 << 1) | (0 << 0))
+#define QUADSPI_BK1_CS_HARDWARE     ((0 << 1) | (1 << 0)) // pin must support QSPI Alternate function for BK1_NCS
+#define QUADSPI_BK1_CS_SOFTWARE     ((1 << 1) | (0 << 0)) // use any GPIO pin for BK1 CS
+
+#define QUADSPI_BK2_CS_MASK         ((1 << 3) | (1 << 2))
+
+#define QUADSPI_BK2_CS_NONE         ((0 << 3) | (0 << 2))
+#define QUADSPI_BK2_CS_HARDWARE     ((0 << 3) | (1 << 2)) // pin must support QSPI Alternate function for BK2_NCS
+#define QUADSPI_BK2_CS_SOFTWARE     ((1 << 3) | (0 << 2)) // use any GPIO pin for BK2 CS
+
+#define QUADSPI_CS_MODE_MASK         (1 << 4)
+
+#define QUADSPI_CS_MODE_SEPARATE    (0 << 4)
+#define QUADSPI_CS_MODE_LINKED      (1 << 4)
+
+// H7 QSPI notes:
+// Hardware supports BK1 and BK2 connected to both flash chips when using DUAL FLASH mode.
+// Hardware does NOT support using BK1_NCS for single flash chip on BK2.
+// It's possible to use BK1_NCS for single chip on BK2 using software CS via QUADSPI_BK2_CS_SOFTWARE
+
+
+void quadSpiPreInit(void);
+
+bool quadSpiInit(QUADSPIDevice device);
+void quadSpiSetDivisor(QUADSPI_TypeDef *instance, uint16_t divisor);
+
+bool quadSpiTransmit1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length);
+bool quadSpiReceive1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length);
+bool quadSpiReceive4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length);
+
+bool quadSpiInstructionWithData1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length);
+
+bool quadSpiReceiveWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length);
+bool quadSpiReceiveWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length);
+bool quadSpiTransmitWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length);
+bool quadSpiTransmitWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length);
+
+
+bool quadSpiInstructionWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize);
+
+//bool quadSpiIsBusBusy(SPI_TypeDef *instance);
+
+uint16_t quadSpiGetErrorCounter(QUADSPI_TypeDef *instance);
+void quadSpiResetErrorCounter(QUADSPI_TypeDef *instance);
+
+QUADSPIDevice quadSpiDeviceByInstance(QUADSPI_TypeDef *instance);
+QUADSPI_TypeDef *quadSpiInstanceByDevice(QUADSPIDevice device);
+
+//
+// Config
+//
+
+typedef struct quadSpiConfig_s {
+    QUADSPIDevice device;
+    ioTag_t clk;
+
+    // Note: Either or both CS pin may be used in DUAL_FLASH mode, any unused pins should be IO_NONE
+    ioTag_t bk1IO0;
+    ioTag_t bk1IO1;
+    ioTag_t bk1IO2;
+    ioTag_t bk1IO3;
+    ioTag_t bk1CS;
+
+    ioTag_t bk2IO0;
+    ioTag_t bk2IO1;
+    ioTag_t bk2IO2;
+    ioTag_t bk2IO3;
+    ioTag_t bk2CS;
+
+    uint8_t mode;
+
+    // CS pins can be under software control, useful when using BK1CS as the CS pin for BK2 in non-DUAL-FLASH mode.
+    uint8_t csFlags;
+} quadSpiConfig_t;
+
+const quadSpiConfig_t * getQuadSpiConfig(QUADSPIDevice device);
+void quadSpiPinConfigure(QUADSPIDevice device);
+
+#endif
--- a/src/main/drivers/bus_quadspi_hal.c
+++ b/src/main/drivers/bus_quadspi_hal.c
@ -0,0 +1,575 @@
+/*
+ * 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/>.
+ *
+ * Author: Dominic Clifton
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_QUADSPI
+
+#include "bus_quadspi.h"
+#include "bus_quadspi_impl.h"
+#include "dma.h"
+#include "io.h"
+#include "io_impl.h"
+#include "nvic.h"
+#include "rcc.h"
+
+static void Error_Handler(void) { while (1) { } }
+
+void quadSpiInitDevice(QUADSPIDevice device)
+{
+    quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]);
+
+    // Enable QUADSPI clock
+    RCC_ClockCmd(quadSpi->rcc, ENABLE);
+    //RCC_ResetCmd(quadSpi->rcc, ENABLE);
+
+    IOInit(IOGetByTag(quadSpi->clk), OWNER_QUADSPI,  RESOURCE_QUADSPI_CLK, RESOURCE_INDEX(device));
+    IOInit(IOGetByTag(quadSpi->bk1IO0), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO0, RESOURCE_INDEX(device));
+    IOInit(IOGetByTag(quadSpi->bk1IO1), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO1, RESOURCE_INDEX(device));
+    IOInit(IOGetByTag(quadSpi->bk1IO2), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO2, RESOURCE_INDEX(device));
+    IOInit(IOGetByTag(quadSpi->bk1IO3), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1IO3, RESOURCE_INDEX(device));
+    IOInit(IOGetByTag(quadSpi->bk1CS), OWNER_QUADSPI, RESOURCE_QUADSPI_BK1CS, RESOURCE_INDEX(device));
+
+    IOInit(IOGetByTag(quadSpi->bk2IO0), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO0, RESOURCE_INDEX(device));
+    IOInit(IOGetByTag(quadSpi->bk2IO1), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO1, RESOURCE_INDEX(device));
+    IOInit(IOGetByTag(quadSpi->bk2IO2), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO2, RESOURCE_INDEX(device));
+    IOInit(IOGetByTag(quadSpi->bk2IO3), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2IO3, RESOURCE_INDEX(device));
+    IOInit(IOGetByTag(quadSpi->bk2CS), OWNER_QUADSPI, RESOURCE_QUADSPI_BK2CS, RESOURCE_INDEX(device));
+
+#if defined(STM32H7)
+    // clock is only on AF9
+    // IO and CS lines are on AF9 and AF10
+    IOConfigGPIOAF(IOGetByTag(quadSpi->clk), QUADSPI_IO_AF_CLK_CFG, GPIO_AF9_QUADSPI);
+    IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO0), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO0AF);
+    IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO1), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO1AF);
+    IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO2), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO2AF);
+    IOConfigGPIOAF(IOGetByTag(quadSpi->bk1IO3), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk1IO3AF);
+    IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO0), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO0AF);
+    IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO1), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO1AF);
+    IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO2), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO2AF);
+    IOConfigGPIOAF(IOGetByTag(quadSpi->bk2IO3), QUADSPI_IO_AF_BK_IO_CFG, quadSpi->bk2IO3AF);
+
+    if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_HARDWARE) {
+        IOConfigGPIOAF(IOGetByTag(quadSpi->bk1CS), QUADSPI_IO_AF_BK_CS_CFG, quadSpi->bk1CSAF);
+    } else {
+        IOConfigGPIO(IOGetByTag(quadSpi->bk1CS), QUADSPI_IO_BK_CS_CFG);
+    }
+
+    if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_HARDWARE) {
+        IOConfigGPIOAF(IOGetByTag(quadSpi->bk2CS), QUADSPI_IO_AF_BK_CS_CFG, quadSpi->bk2CSAF);
+    } else {
+        IOConfigGPIO(IOGetByTag(quadSpi->bk2CS), QUADSPI_IO_BK_CS_CFG);
+    }
+#endif
+
+    quadSpi->hquadSpi.Instance = quadSpi->dev;
+    // DeInit QUADSPI hardware
+    HAL_QSPI_DeInit(&quadSpi->hquadSpi);
+
+    quadSpi->hquadSpi.Init.ClockPrescaler = QUADSPI_CLOCK_INITIALISATION;
+    quadSpi->hquadSpi.Init.FifoThreshold = 1;
+    quadSpi->hquadSpi.Init.SampleShifting = QSPI_SAMPLE_SHIFTING_NONE;
+    quadSpi->hquadSpi.Init.FlashSize = 23; // address bits + 1
+    quadSpi->hquadSpi.Init.ChipSelectHighTime = QSPI_CS_HIGH_TIME_1_CYCLE;
+    quadSpi->hquadSpi.Init.ClockMode = QSPI_CLOCK_MODE_0;
+
+    switch (quadSpi->mode) {
+    case QUADSPI_MODE_BK1_ONLY:
+        quadSpi->hquadSpi.Init.FlashID = QSPI_FLASH_ID_1;
+        quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_DISABLE;
+        break;
+    case QUADSPI_MODE_BK2_ONLY:
+        quadSpi->hquadSpi.Init.FlashID = QSPI_FLASH_ID_2;
+        quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_DISABLE;
+        break;
+    case QUADSPI_MODE_DUAL_FLASH:
+        quadSpi->hquadSpi.Init.DualFlash = QSPI_DUALFLASH_ENABLE;
+        break;
+    }
+
+    // Init QUADSPI hardware
+    if (HAL_QSPI_Init(&quadSpi->hquadSpi) != HAL_OK)
+    {
+      Error_Handler();
+    }
+}
+
+static const uint32_t quadSpi_addressSizeMap[] = {
+    QSPI_ADDRESS_8_BITS,
+    QSPI_ADDRESS_16_BITS,
+    QSPI_ADDRESS_24_BITS,
+    QSPI_ADDRESS_32_BITS
+};
+
+static uint32_t quadSpi_addressSizeFromValue(uint8_t addressSize)
+{
+    return quadSpi_addressSizeMap[((addressSize + 1) / 8) - 1]; // rounds to nearest QSPI_ADDRESS_* value that will hold the address.
+}
+
+/**
+ * Return true if the bus is currently in the middle of a transmission.
+ */
+bool quadSpiIsBusBusy(QUADSPI_TypeDef *instance)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    if(quadSpiDevice[device].hquadSpi.State == HAL_QSPI_STATE_BUSY)
+        return true;
+    else
+        return false;
+}
+
+#define QUADSPI_DEFAULT_TIMEOUT 10
+
+void quadSpiSelectDevice(QUADSPI_TypeDef *instance)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+
+    quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]);
+
+    IO_t bk1CS = IOGetByTag(quadSpi->bk1CS);
+    IO_t bk2CS = IOGetByTag(quadSpi->bk2CS);
+
+    switch(quadSpi->mode) {
+    case QUADSPI_MODE_DUAL_FLASH:
+        if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
+            IOLo(bk1CS);
+        }
+        if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
+            IOLo(bk2CS);
+        }
+        break;
+    case QUADSPI_MODE_BK1_ONLY:
+        if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
+            IOLo(bk1CS);
+        }
+        break;
+    case QUADSPI_MODE_BK2_ONLY:
+        if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
+            IOLo(bk2CS);
+        }
+        break;
+    }
+}
+
+void quadSpiDeselectDevice(QUADSPI_TypeDef *instance)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+
+    quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]);
+
+    IO_t bk1CS = IOGetByTag(quadSpi->bk1CS);
+    IO_t bk2CS = IOGetByTag(quadSpi->bk2CS);
+
+    switch(quadSpi->mode) {
+    case QUADSPI_MODE_DUAL_FLASH:
+        if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
+            IOHi(bk1CS);
+        }
+        if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
+            IOHi(bk2CS);
+        }
+        break;
+    case QUADSPI_MODE_BK1_ONLY:
+        if ((quadSpi->csFlags & QUADSPI_BK1_CS_MASK) == QUADSPI_BK1_CS_SOFTWARE) {
+            IOHi(bk1CS);
+        }
+        break;
+    case QUADSPI_MODE_BK2_ONLY:
+        if ((quadSpi->csFlags & QUADSPI_BK2_CS_MASK) == QUADSPI_BK2_CS_SOFTWARE) {
+            IOHi(bk2CS);
+        }
+        break;
+    }
+}
+
+
+
+bool quadSpiTransmit1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    HAL_StatusTypeDef status;
+
+
+    QSPI_CommandTypeDef cmd;
+    cmd.InstructionMode   = QSPI_INSTRUCTION_1_LINE;
+    cmd.AddressMode       = QSPI_ADDRESS_NONE;
+    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+    cmd.DataMode          = QSPI_DATA_NONE;
+    cmd.DummyCycles       = dummyCycles;
+    cmd.DdrMode           = QSPI_DDR_MODE_DISABLE;
+    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
+    cmd.SIOOMode          = QSPI_SIOO_INST_EVERY_CMD;
+
+    cmd.Instruction       = instruction;
+    cmd.NbData            = length;
+
+    if (out) {
+        cmd.DataMode          = QSPI_DATA_1_LINE;
+    }
+
+    quadSpiSelectDevice(instance);
+
+    status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
+    bool timeout = (status != HAL_OK);
+    if (!timeout) {
+        if (out && length > 0) {
+            status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT);
+            timeout = (status != HAL_OK);
+        }
+    }
+
+    quadSpiDeselectDevice(instance);
+
+    if (timeout) {
+        quadSpiTimeoutUserCallback(instance);
+        return false;
+    }
+
+    return true;
+}
+
+bool quadSpiReceive1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    HAL_StatusTypeDef status;
+
+    QSPI_CommandTypeDef cmd;
+    cmd.InstructionMode   = QSPI_INSTRUCTION_1_LINE;
+    cmd.AddressMode       = QSPI_ADDRESS_NONE;
+    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+    cmd.DataMode          = QSPI_DATA_1_LINE;
+    cmd.DummyCycles       = dummyCycles;
+    cmd.DdrMode           = QSPI_DDR_MODE_DISABLE;
+    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
+    cmd.SIOOMode          = QSPI_SIOO_INST_EVERY_CMD;
+
+    cmd.Instruction       = instruction;
+    cmd.NbData            = length;
+
+    quadSpiSelectDevice(instance);
+
+    status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
+    bool timeout = (status != HAL_OK);
+    if (!timeout) {
+        status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT);
+
+        timeout = (status != HAL_OK);
+    }
+
+    quadSpiDeselectDevice(instance);
+
+    if (timeout) {
+        quadSpiTimeoutUserCallback(instance);
+        return false;
+    }
+
+    return true;
+}
+
+bool quadSpiReceive4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    HAL_StatusTypeDef status;
+
+    QSPI_CommandTypeDef cmd;
+    cmd.InstructionMode   = QSPI_INSTRUCTION_4_LINES;
+    cmd.AddressMode       = QSPI_ADDRESS_NONE;
+    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+    cmd.DataMode          = QSPI_DATA_4_LINES;
+    cmd.DummyCycles       = dummyCycles;
+    cmd.DdrMode           = QSPI_DDR_MODE_DISABLE;
+    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
+    cmd.SIOOMode          = QSPI_SIOO_INST_EVERY_CMD;
+
+    cmd.Instruction       = instruction;
+    cmd.NbData            = length;
+
+    quadSpiSelectDevice(instance);
+
+    status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
+    bool timeout = (status != HAL_OK);
+    if (!timeout) {
+        status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT);
+
+        timeout = (status != HAL_OK);
+    }
+
+    quadSpiDeselectDevice(instance);
+
+    if (timeout) {
+        quadSpiTimeoutUserCallback(instance);
+        return false;
+    }
+
+    return true;
+}
+
+bool quadSpiReceiveWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    HAL_StatusTypeDef status;
+
+    QSPI_CommandTypeDef cmd;
+    cmd.InstructionMode   = QSPI_INSTRUCTION_1_LINE;
+    cmd.AddressMode       = QSPI_ADDRESS_1_LINE;
+    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+    cmd.DataMode          = QSPI_DATA_1_LINE;
+    cmd.DummyCycles       = dummyCycles;
+    cmd.DdrMode           = QSPI_DDR_MODE_DISABLE;
+    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
+    cmd.SIOOMode          = QSPI_SIOO_INST_EVERY_CMD;
+
+    cmd.Instruction       = instruction;
+    cmd.Address           = address;
+    cmd.AddressSize       = quadSpi_addressSizeFromValue(addressSize);
+    cmd.NbData            = length;
+
+    quadSpiSelectDevice(instance);
+
+    status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
+    bool timeout = (status != HAL_OK);
+    if (!timeout) {
+        status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT);
+        timeout = (status != HAL_OK);
+    }
+
+    quadSpiDeselectDevice(instance);
+
+    if (timeout) {
+        quadSpiTimeoutUserCallback(instance);
+        return false;
+    }
+
+    return true;
+}
+
+
+bool quadSpiReceiveWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    HAL_StatusTypeDef status;
+
+    QSPI_CommandTypeDef cmd;
+    cmd.InstructionMode   = QSPI_INSTRUCTION_1_LINE;
+    cmd.AddressMode       = QSPI_ADDRESS_1_LINE;
+    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+    cmd.DataMode          = QSPI_DATA_4_LINES;
+    cmd.DummyCycles       = dummyCycles;
+    cmd.DdrMode           = QSPI_DDR_MODE_DISABLE;
+    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
+    cmd.SIOOMode          = QSPI_SIOO_INST_EVERY_CMD;
+
+    cmd.Instruction       = instruction;
+    cmd.Address           = address;
+    cmd.AddressSize       = quadSpi_addressSizeFromValue(addressSize);
+    cmd.NbData            = length;
+
+    quadSpiSelectDevice(instance);
+
+    status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
+    bool timeout = (status != HAL_OK);
+    if (!timeout) {
+        status = HAL_QSPI_Receive(&quadSpiDevice[device].hquadSpi, in, QUADSPI_DEFAULT_TIMEOUT);
+        timeout = (status != HAL_OK);
+    }
+
+    quadSpiDeselectDevice(instance);
+
+    if (timeout) {
+        quadSpiTimeoutUserCallback(instance);
+        return false;
+    }
+
+    return true;
+}
+bool quadSpiTransmitWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    HAL_StatusTypeDef status;
+
+    QSPI_CommandTypeDef cmd;
+    cmd.InstructionMode   = QSPI_INSTRUCTION_1_LINE;
+    cmd.AddressMode       = QSPI_ADDRESS_1_LINE;
+    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+    cmd.DataMode          = QSPI_DATA_1_LINE;
+    cmd.DummyCycles       = dummyCycles;
+    cmd.DdrMode           = QSPI_DDR_MODE_DISABLE;
+    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
+    cmd.SIOOMode          = QSPI_SIOO_INST_EVERY_CMD;
+
+    cmd.Instruction       = instruction;
+    cmd.Address           = address;
+    cmd.AddressSize       = quadSpi_addressSizeFromValue(addressSize);
+    cmd.NbData            = length;
+
+    quadSpiSelectDevice(instance);
+
+    status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
+    bool timeout = (status != HAL_OK);
+
+    if (!timeout) {
+        status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT);
+        timeout = (status != HAL_OK);
+    }
+
+    quadSpiDeselectDevice(instance);
+
+    if (timeout) {
+        quadSpiTimeoutUserCallback(instance);
+        return false;
+    }
+
+    return true;
+}
+
+bool quadSpiTransmitWithAddress4LINES(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    HAL_StatusTypeDef status;
+
+    QSPI_CommandTypeDef cmd;
+    cmd.InstructionMode   = QSPI_INSTRUCTION_1_LINE;
+    cmd.AddressMode       = QSPI_ADDRESS_1_LINE;
+    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+    cmd.DataMode          = QSPI_DATA_4_LINES;
+    cmd.DummyCycles       = dummyCycles;
+    cmd.DdrMode           = QSPI_DDR_MODE_DISABLE;
+    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
+    cmd.SIOOMode          = QSPI_SIOO_INST_EVERY_CMD;
+
+    cmd.Instruction       = instruction;
+    cmd.Address           = address;
+    cmd.AddressSize       = quadSpi_addressSizeFromValue(addressSize);
+    cmd.NbData            = length;
+
+    quadSpiSelectDevice(instance);
+
+    status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
+    bool timeout = (status != HAL_OK);
+
+    if (!timeout) {
+        status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT);
+        timeout = (status != HAL_OK);
+    }
+
+    quadSpiDeselectDevice(instance);
+
+    if (timeout) {
+        quadSpiTimeoutUserCallback(instance);
+        return false;
+    }
+
+    return true;
+}
+
+bool quadSpiInstructionWithAddress1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    HAL_StatusTypeDef status;
+
+    QSPI_CommandTypeDef cmd;
+    cmd.InstructionMode   = QSPI_INSTRUCTION_1_LINE;
+    cmd.AddressMode       = QSPI_ADDRESS_1_LINE;
+    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+    cmd.DataMode          = QSPI_DATA_NONE;
+    cmd.DummyCycles       = dummyCycles;
+    cmd.DdrMode           = QSPI_DDR_MODE_DISABLE;
+    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
+    cmd.SIOOMode          = QSPI_SIOO_INST_EVERY_CMD;
+
+    cmd.Instruction       = instruction;
+    cmd.Address           = address;
+    cmd.AddressSize       = quadSpi_addressSizeFromValue(addressSize);
+    cmd.NbData            = 0;
+
+    quadSpiSelectDevice(instance);
+
+    status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
+    bool timeout = (status != HAL_OK);
+
+    quadSpiDeselectDevice(instance);
+
+    if (timeout) {
+        quadSpiTimeoutUserCallback(instance);
+        return false;
+    }
+
+
+    return true;
+}
+
+bool quadSpiInstructionWithData1LINE(QUADSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    HAL_StatusTypeDef status;
+
+    QSPI_CommandTypeDef cmd;
+    cmd.InstructionMode   = QSPI_INSTRUCTION_1_LINE;
+    cmd.AddressMode       = QSPI_ADDRESS_NONE;
+    cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+    cmd.DataMode          = QSPI_DATA_1_LINE;
+    cmd.DummyCycles       = dummyCycles;
+    cmd.DdrMode           = QSPI_DDR_MODE_DISABLE;
+    cmd.DdrHoldHalfCycle  = QSPI_DDR_HHC_ANALOG_DELAY;
+    cmd.SIOOMode          = QSPI_SIOO_INST_EVERY_CMD;
+
+    cmd.Instruction       = instruction;
+    cmd.NbData            = length;
+
+    quadSpiSelectDevice(instance);
+
+    status = HAL_QSPI_Command(&quadSpiDevice[device].hquadSpi, &cmd, QUADSPI_DEFAULT_TIMEOUT);
+    bool timeout =(status != HAL_OK);
+
+    if (!timeout) {
+        status = HAL_QSPI_Transmit(&quadSpiDevice[device].hquadSpi, (uint8_t *)out, QUADSPI_DEFAULT_TIMEOUT);
+        timeout = (status != HAL_OK);
+    }
+
+    quadSpiDeselectDevice(instance);
+
+    if (timeout) {
+        quadSpiTimeoutUserCallback(instance);
+        return false;
+    }
+
+    return true;
+}
+
+void quadSpiSetDivisor(QUADSPI_TypeDef *instance, uint16_t divisor)
+{
+    QUADSPIDevice device = quadSpiDeviceByInstance(instance);
+    if (HAL_QSPI_DeInit(&quadSpiDevice[device].hquadSpi) != HAL_OK)
+    {
+        Error_Handler();
+    }
+
+    quadSpiDevice_t *quadSpi = &(quadSpiDevice[device]);
+
+    quadSpi->hquadSpi.Init.ClockPrescaler = divisor;
+
+    HAL_QSPI_Init(&quadSpi->hquadSpi);
+}
+#endif
--- a/src/main/drivers/bus_quadspi_impl.h
+++ b/src/main/drivers/bus_quadspi_impl.h
@ -0,0 +1,94 @@
+/*
+ * 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/>.
+ *
+ * Author: Dominic Clifton
+ */
+
+#pragma once
+
+
+typedef struct quadSpiPinDef_s {
+    ioTag_t pin;
+#if defined(STM32H7)
+    uint8_t af;
+#endif
+} quadSpiPinDef_t;
+
+#if defined(STM32H7)
+#define MAX_QUADSPI_PIN_SEL 3
+#endif
+
+typedef struct quadSpiHardware_s {
+    QUADSPIDevice device;
+    QUADSPI_TypeDef *reg;
+    quadSpiPinDef_t clkPins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk1IO0Pins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk1IO1Pins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk1IO2Pins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk1IO3Pins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk1CSPins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk2IO0Pins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk2IO1Pins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk2IO2Pins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk2IO3Pins[MAX_QUADSPI_PIN_SEL];
+    quadSpiPinDef_t bk2CSPins[MAX_QUADSPI_PIN_SEL];
+
+    rccPeriphTag_t rcc;
+} quadSpiHardware_t;
+
+extern const quadSpiHardware_t quadSpiHardware[];
+
+typedef struct QUADSPIDevice_s {
+    QUADSPI_TypeDef *dev;
+    ioTag_t clk;
+    ioTag_t bk1IO0;
+    ioTag_t bk1IO1;
+    ioTag_t bk1IO2;
+    ioTag_t bk1IO3;
+    ioTag_t bk1CS;
+    ioTag_t bk2IO0;
+    ioTag_t bk2IO1;
+    ioTag_t bk2IO2;
+    ioTag_t bk2IO3;
+    ioTag_t bk2CS;
+#if defined(STM32H7)
+    uint8_t bk1IO0AF;
+    uint8_t bk1IO1AF;
+    uint8_t bk1IO2AF;
+    uint8_t bk1IO3AF;
+    uint8_t bk1CSAF;
+    uint8_t bk2IO0AF;
+    uint8_t bk2IO1AF;
+    uint8_t bk2IO2AF;
+    uint8_t bk2IO3AF;
+    uint8_t bk2CSAF;
+#endif
+    uint8_t mode;
+    uint8_t csFlags;
+    rccPeriphTag_t rcc;
+    volatile uint16_t errorCount;
+#if defined(USE_HAL_DRIVER)
+    QSPI_HandleTypeDef hquadSpi;
+#endif
+} quadSpiDevice_t;
+
+extern quadSpiDevice_t quadSpiDevice[QUADSPIDEV_COUNT];
+
+void quadSpiInitDevice(QUADSPIDevice device);
+uint32_t quadSpiTimeoutUserCallback(QUADSPI_TypeDef *instance);
--- a/src/main/drivers/bus_spi_at32f43x.c
+++ b/src/main/drivers/bus_spi_at32f43x.c
@ -87,7 +87,7 @@
        { .dev = NULL },    // No SPI1
    #endif
    #ifdef USE_SPI_DEVICE_2
-        { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_MUX_6, .divisorMap = spiDivisorMapSlow },
+        { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_MUX_5, .divisorMap = spiDivisorMapSlow },
    #else
        { .dev = NULL },    // No SPI2
    #endif
--- a/src/main/drivers/compass/compass_msp.c
+++ b/src/main/drivers/compass/compass_msp.c
@ -45,7 +45,7 @@

 #define MSP_MAG_TIMEOUT_MS      250     // Less than 4Hz updates is considered a failure

-static int32_t mspMagData[XYZ_AXIS_COUNT];
+static float mspMagData[XYZ_AXIS_COUNT];
 static timeMs_t mspMagLastUpdateMs;

 static bool mspMagInit(magDev_t *magDev)
@ -62,9 +62,9 @@ void mspMagReceiveNewData(uint8_t * bufferPtr)
 {
    const mspSensorCompassDataMessage_t * pkt = (const mspSensorCompassDataMessage_t *)bufferPtr;

-    mspMagData[X] = pkt->magX;
-    mspMagData[Y] = pkt->magY;
-    mspMagData[Z] = pkt->magZ;
+    mspMagData[X] = (float) pkt->magX;
+    mspMagData[Y] = (float) pkt->magY;
+    mspMagData[Z] = (float) pkt->magZ;

    applySensorAlignment(mspMagData, mspMagData, CW90_DEG_FLIP);

--- a/src/main/drivers/flash.c
+++ b/src/main/drivers/flash.c
@ -218,7 +218,7 @@ static void flashConfigurePartitions(void)
 #endif
 }

-flashPartition_t *flashPartitionFindByType(uint8_t type)
+flashPartition_t *flashPartitionFindByType(flashPartitionType_e type)
 {
    for (int index = 0; index < FLASH_MAX_PARTITIONS; index++) {
        flashPartition_t *candidate = &flashPartitionTable.partitions[index];
--- a/src/main/drivers/flash_m25p16.c
+++ b/src/main/drivers/flash_m25p16.c
@ -27,13 +27,20 @@
 #include "drivers/bus.h"
 #include "drivers/time.h"

+#if defined(USE_QUADSPI)
+#include "drivers/bus_quadspi.h"
+#include "drivers/bus_quadspi_impl.h"
+#endif
+
 #define M25P16_INSTRUCTION_RDID             0x9F
 #define M25P16_INSTRUCTION_READ_BYTES       0x03
+#define M25P16_INSTRUCTION_QUAD_READ        0x6B
 #define M25P16_INSTRUCTION_READ_STATUS_REG  0x05
 #define M25P16_INSTRUCTION_WRITE_STATUS_REG 0x01
 #define M25P16_INSTRUCTION_WRITE_ENABLE     0x06
 #define M25P16_INSTRUCTION_WRITE_DISABLE    0x04
 #define M25P16_INSTRUCTION_PAGE_PROGRAM     0x02
+#define M25P16_INSTRUCTION_QPAGE_PROGRAM    0x32
 #define M25P16_INSTRUCTION_SECTOR_ERASE     0xD8
 #define M25P16_INSTRUCTION_BULK_ERASE       0xC7

@ -42,6 +49,8 @@

 #define W25Q256_INSTRUCTION_ENTER_4BYTE_ADDRESS_MODE 0xB7

+#define M25P16_FAST_READ_DUMMY_CYCLES       8
+
 struct {
    uint32_t        jedecID;
    flashSector_t   sectors;
@ -118,7 +127,12 @@ struct {

 static flashGeometry_t geometry = {.pageSize = M25P16_PAGESIZE};

+#if !defined(M25P16_QUADSPI_DEVICE)
 static busDevice_t * busDev = NULL;
+#else
+static QUADSPI_TypeDef * qspi = NULL;
+#endif /* !defined(M25P16_QUADSPI_DEVICE) */
+
 static bool isLargeFlash = false;
 static uint32_t timeoutAt = 0;

@ -134,7 +148,11 @@ static bool couldBeBusy = false;
 */
 static void m25p16_performOneByteCommand(uint8_t command)
 {
+#if !defined(M25P16_QUADSPI_DEVICE)
    busTransfer(busDev, NULL, &command, 1);
+#else
+    quadSpiTransmit1LINE(qspi, command, 0, NULL, 0);
+#endif /* !defined(M25P16_QUADSPI_DEVICE) */
 }

 /**
@ -151,12 +169,18 @@ static void m25p16_writeEnable(void)

 static uint8_t m25p16_readStatus(void)
 {
+    uint8_t status;
+#if !defined(M25P16_QUADSPI_DEVICE)
    uint8_t command[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 };
    uint8_t in[2];

    busTransfer(busDev, in, command, sizeof(command));
+    status = in[1];
+#else
+    quadSpiReceive1LINE(qspi, M25P16_INSTRUCTION_READ_STATUS_REG, 0, &status, 1);
+#endif /* !defined(M25P16_QUADSPI_DEVICE) */

-    return in[1];
+    return status;
 }

 bool m25p16_isReady(void)
@ -201,7 +225,6 @@ bool m25p16_waitForReady(uint32_t timeoutMillis)
 */
 static bool m25p16_readIdentification(void)
 {
-    uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0 };
    uint8_t in[4];
    uint32_t chipID;

@ -212,7 +235,16 @@ static bool m25p16_readIdentification(void)
     */
    in[1] = 0;

+#if !defined(M25P16_QUADSPI_DEVICE)
+    uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0 };
+
    busTransfer(busDev, in, out, sizeof(out));
+#else
+    bool status = quadSpiReceive1LINE(qspi, M25P16_INSTRUCTION_RDID, 0, &in[1], sizeof(in) - 1);
+    if (!status) {
+        return false;
+    }
+#endif /* !defined(M25P16_QUADSPI_DEVICE) */

    // Manufacturer, memory type, and capacity
    chipID = (in[1] << 16) | (in[2] << 8) | (in[3]);
@ -256,6 +288,9 @@ static bool m25p16_readIdentification(void)
 */
 bool m25p16_init(int flashNumToUse)
 {
+    bool detected;
+#if !defined(M25P16_QUADSPI_DEVICE)
+    // SPI Mode
    busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_M25P16, flashNumToUse, OWNER_FLASH);
    if (busDev == NULL) {
        return false;
@ -264,8 +299,25 @@ bool m25p16_init(int flashNumToUse)
 #ifndef M25P16_SPI_SHARED
    busSetSpeed(busDev, BUS_SPEED_FAST);
 #endif
+#else
+    // QUADSPI Mode
+    UNUSED(flashNumToUse);
+    quadSpiPinConfigure(M25P16_QUADSPI_DEVICE);
+    quadSpiInitDevice(M25P16_QUADSPI_DEVICE);

-    return m25p16_readIdentification();
+    qspi = quadSpiInstanceByDevice(M25P16_QUADSPI_DEVICE);
+    quadSpiSetDivisor(qspi, QUADSPI_CLOCK_INITIALISATION);
+#endif /* M25P16_QUADSPI_DEVICE */
+
+    detected = m25p16_readIdentification();
+
+#if defined(M25P16_QUADSPI_DEVICE)
+    if (detected) {
+        quadSpiSetDivisor(qspi, QUADSPI_CLOCK_ULTRAFAST);
+    }
+#endif
+
+    return detected;
 }

 void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLongAddress)
@ -284,17 +336,20 @@ void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLongAddres
 */
 void m25p16_eraseSector(uint32_t address)
 {
-    uint8_t out[5] = { M25P16_INSTRUCTION_SECTOR_ERASE };
-
-    m25p16_setCommandAddress(&out[1], address, isLargeFlash);
-
    if (!m25p16_waitForReady(0)) {
        return;
    }

    m25p16_writeEnable();

+#if !defined(M25P16_QUADSPI_DEVICE)
+    uint8_t out[5] = { M25P16_INSTRUCTION_SECTOR_ERASE };
+    m25p16_setCommandAddress(&out[1], address, isLargeFlash);
+
    busTransfer(busDev, NULL, out, isLargeFlash ? 5 : 4);
+#else
+    quadSpiInstructionWithAddress1LINE(qspi, M25P16_INSTRUCTION_SECTOR_ERASE, 0, address, isLargeFlash ? 32 : 24);
+#endif /* !defined(M25P16_QUADSPI_DEVICE) */

    m25p16_setTimeout(SECTOR_ERASE_TIMEOUT_MILLIS);
 }
@ -329,6 +384,14 @@ void m25p16_eraseCompletely(void)
 */
 uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)
 {
+    if (!m25p16_waitForReady(0)) {
+        // return same address to indicate timeout
+        return address;
+    }
+
+    m25p16_writeEnable();
+
+#if !defined(M25P16_QUADSPI_DEVICE)
    uint8_t command[5] = { M25P16_INSTRUCTION_PAGE_PROGRAM };

    busTransferDescriptor_t txn[2] = {
@ -338,14 +401,10 @@ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)

    m25p16_setCommandAddress(&command[1], address, isLargeFlash);

-    if (!m25p16_waitForReady(0)) {
-        // return same address to indicate timeout
-        return address;
-    }
-
-    m25p16_writeEnable();
-
    busTransferMultiple(busDev, txn, 2);
+#else
+    quadSpiTransmitWithAddress4LINES(qspi, M25P16_INSTRUCTION_QPAGE_PROGRAM, 0, address, isLargeFlash ? 32 : 24, data, length);
+#endif /* !defined(M25P16_QUADSPI_DEVICE) */

    m25p16_setTimeout(DEFAULT_TIMEOUT_MILLIS);

@ -362,6 +421,11 @@ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)
 */
 int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length)
 {
+    if (!m25p16_waitForReady(0)) {
+        return 0;
+    }
+
+#if !defined(M25P16_QUADSPI_DEVICE)
    uint8_t command[5] = { M25P16_INSTRUCTION_READ_BYTES };

    busTransferDescriptor_t txn[2] = {
@ -371,11 +435,11 @@ int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length)

    m25p16_setCommandAddress(&command[1], address, isLargeFlash);

-    if (!m25p16_waitForReady(0)) {
-        return 0;
-    }
-
    busTransferMultiple(busDev, txn, 2);
+#else
+    quadSpiReceiveWithAddress4LINES(qspi, M25P16_INSTRUCTION_QUAD_READ, M25P16_FAST_READ_DUMMY_CYCLES,
+                                    address, isLargeFlash ? 32 : 24, buffer, length);
+#endif /* !defined(M25P16_QUADSPI_DEVICE) */

    return length;
 }
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@ -23,8 +23,6 @@

 #include "platform.h"

-FILE_COMPILE_FOR_SPEED
-
 #ifdef USE_MAX7456

 #if defined(MAX7456_USE_BOUNDS_CHECKS)
--- a/src/main/drivers/opflow/opflow.h
+++ b/src/main/drivers/opflow/opflow.h
@ -31,7 +31,7 @@ struct opflowDev_s;

 typedef struct opflowData_s {
    timeDelta_t deltaTime;      // Integration timeframe of motionX/Y
-    int32_t     flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions
+    float     flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions
    int16_t     quality;
 } opflowData_t;

--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@ -176,6 +176,7 @@
 #define SYM_GROUND_COURSE           0xDC  // 220 Ground course
 #define SYM_ALERT                   0xDD  // 221 General alert symbol

+#define SYM_TERRAIN_FOLLOWING       0xFB  // 251 Terrain following (also Alt adjust)
 #define SYM_CROSS_TRACK_ERROR       0xFC  // 252 Cross track error

 #define SYM_LOGO_START              0x101 // 257 to 297, INAV logo
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@ -195,6 +195,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
        return true;
    }
 #endif
+#if defined(ADC_CHANNEL_5_PIN)
+    if (timHw->tag == IO_TAG(ADC_CHANNEL_5_PIN)) {
+        return true;
+    }
+#endif
+#if defined(ADC_CHANNEL_6_PIN)
+    if (timHw->tag == IO_TAG(ADC_CHANNEL_6_PIN)) {
+        return true;
+    }
+#endif
 #endif

    return false;
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@ -24,8 +24,6 @@

 #if !defined(SITL_BUILD)

-FILE_COMPILE_FOR_SPEED
-
 #include "build/debug.h"

 #include "common/log.h"
--- a/src/main/drivers/rangefinder/rangefinder_vl53l0x.c
+++ b/src/main/drivers/rangefinder/rangefinder_vl53l0x.c
@ -367,7 +367,7 @@ uint16_t encodeTimeout(uint16_t timeout_mclks)
 // Defaults to 0.25 MCPS as initialized by the ST API and this library.
 bool setSignalRateLimit(busDevice_t * busDev, float limit_Mcps)
 {
-    if (limit_Mcps < 0 || limit_Mcps > 511.99) {
+    if (limit_Mcps < 0 || limit_Mcps > 511.99f) {
        return false;
    }

--- a/src/main/drivers/resource.c
+++ b/src/main/drivers/resource.c
@ -19,7 +19,7 @@

 const char * const ownerNames[OWNER_TOTAL_COUNT] = {
    "FREE", "PWM/IO", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "TIMER",
-    "RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
+    "RANGEFINDER", "SYSTEM", "SPI", "QUADSPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
    "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER",
    "VTX", "SPI_PREINIT", "COMPASS", "TEMPERATURE", "1-WIRE", "AIRSPEED", "OLED DISPLAY",
    "PINIO", "IRLOCK"
@ -33,6 +33,8 @@ const char * const resourceNames[RESOURCE_TOTAL_COUNT] = {
    "EXTI",
    "SCL", "SDA",
    "SCK", "MOSI", "MISO", "CS",
+    "CLK","BK1IO0", "BK1IO1", "BK1IO2", "BK1IO3", "BK1CS",
+    "BK2IO0", "BK2IO1", "BK2IO2", "BK2IO3", "BK2CS",
    "CH1", "CH2", "CH3", "CH4",
    "CE"
 };
--- a/src/main/drivers/resource.h
+++ b/src/main/drivers/resource.h
@ -31,6 +31,7 @@ typedef enum {
    OWNER_RANGEFINDER,
    OWNER_SYSTEM,
    OWNER_SPI,
+    OWNER_QUADSPI,
    OWNER_I2C,
    OWNER_SDCARD,
    OWNER_FLASH,
@ -68,6 +69,10 @@ typedef enum {
    RESOURCE_EXTI,
    RESOURCE_I2C_SCL, RESOURCE_I2C_SDA,
    RESOURCE_SPI_SCK, RESOURCE_SPI_MOSI, RESOURCE_SPI_MISO, RESOURCE_SPI_CS,
+    RESOURCE_QUADSPI_CLK, RESOURCE_QUADSPI_BK1IO0, RESOURCE_QUADSPI_BK1IO1,
+    RESOURCE_QUADSPI_BK1IO2, RESOURCE_QUADSPI_BK1IO3, RESOURCE_QUADSPI_BK1CS,
+    RESOURCE_QUADSPI_BK2IO0, RESOURCE_QUADSPI_BK2IO1, RESOURCE_QUADSPI_BK2IO2,
+    RESOURCE_QUADSPI_BK2IO3, RESOURCE_QUADSPI_BK2CS,
    RESOURCE_ADC_CH1, RESOURCE_ADC_CH2, RESOURCE_ADC_CH3, RESOURCE_ADC_CH4,
    RESOURCE_RX_CE,
    RESOURCE_TOTAL_COUNT
--- a/src/main/drivers/sdcard/sdcard_spi.c
+++ b/src/main/drivers/sdcard/sdcard_spi.c
@ -90,10 +90,6 @@ static void sdcardSpi_reset(void)
        return;
    }

-    if (sdcard.state >= SDCARD_STATE_READY) {
-        busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION);
-    }
-
    sdcard.failureCount++;
    if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) {
        sdcard.state = SDCARD_STATE_NOT_PRESENT;
@ -474,6 +470,8 @@ static bool sdcardSpi_poll(void)
                    sdcard.state = SDCARD_STATE_NOT_PRESENT;
                }
            }
+
+            busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
        break;

        case SDCARD_STATE_CARD_INIT_IN_PROGRESS:
@ -511,6 +509,8 @@ static bool sdcardSpi_poll(void)
                    }
                }
            }
+
+            busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
        break;
        case SDCARD_STATE_INITIALIZATION_RECEIVE_CID:
            if (sdcardSpi_receiveCID()) {
@ -532,6 +532,8 @@ static bool sdcardSpi_poll(void)
                sdcard.state = SDCARD_STATE_READY;
                goto doMore;
            } // else keep waiting for the CID to arrive
+
+            busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
        break;
        case SDCARD_STATE_SENDING_WRITE:
            // Have we finished sending the write yet?
@ -851,16 +853,18 @@ void sdcardSpi_init(void)
    busSetSpeed(sdcard.dev, BUS_SPEED_INITIALIZATION);

    // SDCard wants 1ms minimum delay after power is applied to it
-    delay(1000);
+    delay(1);

    // Transmit at least 74 dummy clock cycles with CS high so the SD card can start up
-    busDeselectDevice(sdcard.dev);
-    busTransfer(sdcard.dev, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES);
+    IOHi(sdcard.dev->busdev.spi.csnPin);
+    SPI_TypeDef * instance = spiInstanceByDevice(sdcard.dev->busdev.spi.spiBus);
+    spiTransfer(instance, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES);

    // Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles:
    int time = 100000;
    while (busIsBusy(sdcard.dev)) {
        if (time-- == 0) {
+            busSetSpeed(sdcard.dev, BUS_SPEED_STANDARD);
            busDeviceDeInit(sdcard.dev);
            sdcard.dev = NULL;
            sdcard.state = SDCARD_STATE_NOT_PRESENT;
--- a/src/main/drivers/sdcard/sdmmc_sdio_h7xx.c
+++ b/src/main/drivers/sdcard/sdmmc_sdio_h7xx.c
@ -127,14 +127,22 @@ void sdioPinConfigure(void)

    sdioHardware = &sdioPinHardware[SDCARD_SDIO_DEVICE];

-    sdioPin[SDIO_PIN_CK] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCK[0];
-    sdioPin[SDIO_PIN_CMD] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinCMD[0];
-    sdioPin[SDIO_PIN_D0] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD0[0];  
+#ifdef SDCARD_SDIO2_CK_ALT
+    sdioPin[SDIO_PIN_CK] = sdioHardware->sdioPinCK[1];
+#else
+    sdioPin[SDIO_PIN_CK] = sdioHardware->sdioPinCK[0];
+#endif
+#ifdef SDCARD_SDIO2_CMD_ALT
+    sdioPin[SDIO_PIN_CMD] = sdioHardware->sdioPinCMD[1];
+#else
+    sdioPin[SDIO_PIN_CMD] = sdioHardware->sdioPinCMD[0];
+#endif
+    sdioPin[SDIO_PIN_D0] = sdioHardware->sdioPinD0[0];  

 #ifdef SDCARD_SDIO_4BIT
-    sdioPin[SDIO_PIN_D1] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD1[0];
-    sdioPin[SDIO_PIN_D2] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD2[0];
-    sdioPin[SDIO_PIN_D3] = sdioHardware[SDCARD_SDIO_DEVICE].sdioPinD3[0];
+    sdioPin[SDIO_PIN_D1] = sdioHardware->sdioPinD1[0];
+    sdioPin[SDIO_PIN_D2] = sdioHardware->sdioPinD2[0];
+    sdioPin[SDIO_PIN_D3] = sdioHardware->sdioPinD3[0];
 #endif
 }

@ -254,7 +262,11 @@ bool SD_Init(void)
    hsd1.Init.BusWide = SDMMC_BUS_WIDE_1B; // FIXME untested
 #endif
    hsd1.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE;
-    hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see   HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
+#ifdef SDCARD_SDIO_NORMAL_SPEED
+        hsd1.Init.ClockDiv = SDMMC_NSpeed_CLK_DIV;
+#else
+        hsd1.Init.ClockDiv = 1; // 200Mhz / (2 * 1 ) = 100Mhz, used for "UltraHigh speed SD card" only, see   HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
+#endif

    status = HAL_SD_Init(&hsd1); // Will call HAL_SD_MspInit

--- a/src/main/drivers/serial_tcp.c
+++ b/src/main/drivers/serial_tcp.c
@ -37,6 +37,7 @@
 #include <netdb.h>
 #include <arpa/inet.h>
 #include <errno.h>
+#include <netinet/tcp.h>

 #include "common/utils.h"

@ -45,83 +46,14 @@

 static const struct serialPortVTable tcpVTable[];
 static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
-static bool tcpThreadRunning = false;
-
-static int lookup_address (char *name, int port, int type, struct sockaddr *addr, socklen_t* len )
-{
-    struct addrinfo *servinfo, *p;
-    struct addrinfo hints = {.ai_family = AF_UNSPEC, .ai_socktype = type, .ai_flags = AI_V4MAPPED|AI_ADDRCONFIG};
-    if (name == NULL) {
-	hints.ai_flags |= AI_PASSIVE;
-    }
-    /*
-      This nonsense is to uniformly deliver the same sa_family regardless of whether
-      name is NULL or non-NULL
-      Otherwise, at least on Linux, we get
-      - V6,V4 for the non-null case and
-      - V4,V6 for the null case, regardless of gai.conf
-      Which may confuse consumers.
-      FreeBSD and Windows behave consistently, giving V6 for Ipv6 enabled stacks in all cases
-      unless a quad dotted address is specificied
-    */
-    struct addrinfo *p4 = NULL;
-    struct addrinfo *p6 = NULL;
-    int result;
-    char aport[16];
-    snprintf(aport, sizeof(aport), "%d", port);
-    if ((result = getaddrinfo(name, aport, &hints, &servinfo)) != 0) {
-        fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result));
-        return result;
-    } else {
-	int j = 0;
-	for(p = servinfo; p != NULL; p = p->ai_next) {
-	    if(p->ai_family == AF_INET6)
-		p6 = p;
-	    else if(p->ai_family == AF_INET)
-        p4 = p;
-	    j++;
-	}
-	if (p6 != NULL)
-	    p = p6;
-	else if (p4 != NULL)
-	    p = p4;
-	else
-	    return -1;
-	memcpy(addr, p->ai_addr, p->ai_addrlen);
-	*len = p->ai_addrlen;
-	freeaddrinfo(servinfo);
-    }
-    return 0;
-}
-
-static char *tcpGetAddressString(struct sockaddr *addr)
-{
-    static char straddr[INET6_ADDRSTRLEN];
-    void *ipaddr;
-    if (addr->sa_family == AF_INET6) {
-	struct sockaddr_in6 * ip = (struct sockaddr_in6*)addr;
-	ipaddr = &ip->sin6_addr;
-    } else {
-	struct sockaddr_in * ip = (struct sockaddr_in*)addr;
-	ipaddr = &ip->sin_addr;
-    }
-    const char *res = inet_ntop(addr->sa_family, ipaddr, straddr, sizeof straddr);
-    return (char *)res;
-}

 static void *tcpReceiveThread(void* arg)
 {
    tcpPort_t *port = (tcpPort_t*)arg;
-
-    while(tcpThreadRunning) {
-        if (tcpReceive(port) < 0) {
-            break;
-        }
-    }
-
+    while(tcpReceive(port) >= 0)
+        ;
    return NULL;
 }
-
 static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
 {
    socklen_t sockaddrlen;
@ -134,8 +66,8 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
    }

    uint16_t tcpPort = BASE_IP_ADDRESS + id - 1;
-    if (lookup_address(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) {
-	return NULL;
+    if (lookupAddress(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) {
+            return NULL;
    }
    port->socketFd = socket(((struct sockaddr*)&port->sockAddress)->sa_family, SOCK_STREAM, IPPROTO_TCP);

@ -147,15 +79,16 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
 #ifdef __CYGWIN__
    // Sadly necesary to enforce dual-stack behaviour on Windows networking ,,,
    if (((struct sockaddr*)&port->sockAddress)->sa_family == AF_INET6) {
-	int v6only=0;
-	err = setsockopt(port->socketFd, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only));
-	if (err != 0) {
-	    fprintf(stderr,"[SOCKET] setting V6ONLY=false: %s\n", strerror(errno));
-	}
+        int v6only=0;
+        err = setsockopt(port->socketFd, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only));
+        if (err != 0) {
+            fprintf(stderr,"[SOCKET] setting V6ONLY=false: %s\n", strerror(errno));
+        }
    }
 #endif

    int one = 1;
+    err = setsockopt(port->socketFd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
    err = setsockopt(port->socketFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
    err = fcntl(port->socketFd, F_SETFL, fcntl(port->socketFd, F_GETFL, 0) | O_NONBLOCK);

@ -177,15 +110,17 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
        fprintf(stderr, "[SOCKET] Unable to listen.\n");
        return NULL;
    }
-
-    fprintf(stderr, "[SOCKET] Bind TCP %s port %d to UART%d\n",
-	    tcpGetAddressString((struct sockaddr*)&port->sockAddress), tcpPort, id);
-
+    char addrbuf[IPADDRESS_PRINT_BUFLEN];
+    char *addrptr = prettyPrintAddress((struct sockaddr *)&port->sockAddress, addrbuf, IPADDRESS_PRINT_BUFLEN);
+    if (addrptr != NULL) {
+        fprintf(stderr, "[SOCKET] Bind TCP %s to UART%d\n", addrptr, id);
+    }
    return port;
 }

 int tcpReceive(tcpPort_t *port)
 {
+    char addrbuf[IPADDRESS_PRINT_BUFLEN];
    if (!port->isClientConnected) {

        fd_set fds;
@ -198,29 +133,33 @@ int tcpReceive(tcpPort_t *port)
            return -1;
        }

-	socklen_t addrLen = sizeof(struct sockaddr_storage);
+        socklen_t addrLen = sizeof(struct sockaddr_storage);
        port->clientSocketFd = accept(port->socketFd,(struct sockaddr*)&port->clientAddress, &addrLen);
        if (port->clientSocketFd < 1) {
            fprintf(stderr, "[SOCKET] Can't accept connection.\n");
            return -1;
        }

-       fprintf(stderr, "[SOCKET] %s connected to UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id);
+        char *addrptr = prettyPrintAddress((struct sockaddr *)&port->clientAddress, addrbuf, IPADDRESS_PRINT_BUFLEN);
+        if (addrptr != NULL) {
+           fprintf(stderr, "[SOCKET] %s connected to UART%d\n", addrptr, port->id);
+        }
        port->isClientConnected = true;
    }

    uint8_t buffer[TCP_BUFFER_SIZE];
    ssize_t recvSize = recv(port->clientSocketFd, buffer, TCP_BUFFER_SIZE, 0);

-    // Disconnect
-    if (port->isClientConnected && recvSize == 0)
-    {
-       fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id);
-       close(port->clientSocketFd);
-       memset(&port->clientAddress, 0, sizeof(port->clientAddress));
-       port->isClientConnected = false;
-
-       return 0;
+    // recv() under cygwin does not recognise the closed connection under certain circumstances, but returns ECONNRESET as an error.
+    if (port->isClientConnected && (recvSize == 0 || ( recvSize == -1 && errno == ECONNRESET))) {
+        char *addrptr = prettyPrintAddress((struct sockaddr *)&port->clientAddress, addrbuf, IPADDRESS_PRINT_BUFLEN);
+        if (addrptr != NULL) {
+            fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", addrptr, port->id);
+        }
+        close(port->clientSocketFd);
+        memset(&port->clientAddress, 0, sizeof(port->clientAddress));
+        port->isClientConnected = false;
+        return 0;
    }

    for (ssize_t i = 0; i < recvSize; i++) {
@ -248,7 +187,7 @@ serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,

 #if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8)
    uint32_t id = (uintptr_t)USARTx;
-    if (id < SERIAL_PORT_COUNT) {
+    if (id <= SERIAL_PORT_COUNT) {
        port = tcpReConfigure(&tcpPorts[id-1], id);
    }
 #endif
@ -273,8 +212,6 @@ serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
        fprintf(stderr, "[SOCKET] Unable to create receive thread for UART%d\n", id);
        return NULL;
    }
-    tcpThreadRunning = true;
-
    return (serialPort_t*)port;
 }

--- a/src/main/drivers/vtx_common.c
+++ b/src/main/drivers/vtx_common.c
@ -24,6 +24,7 @@

 #include "platform.h"
 #include "build/debug.h"
+#include "common/log.h"

 #include "vtx_common.h"

@ -73,8 +74,9 @@ void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t ch
    if (!vtxDevice)
        return;

-    if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount))
+    if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) {
        return;
+    }

    if (vtxDevice->vTable->setBandAndChannel) {
        vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel);
--- a/src/main/drivers/vtx_common.h
+++ b/src/main/drivers/vtx_common.h
@ -36,7 +36,7 @@
 #define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0             //min freq (in MHz) for 'vtx_freq' setting
 #define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999          //max freq (in MHz) for 'vtx_freq' setting

-#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
+#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)

 #define VTX_SETTINGS_POWER_COUNT        5
 #define VTX_SETTINGS_DEFAULT_POWER      1
@ -63,6 +63,7 @@ typedef enum {
    VTXDEV_SMARTAUDIO = 3,
    VTXDEV_TRAMP      = 4,
    VTXDEV_FFPV       = 5,
+    VTXDEV_MSP        = 6,
    VTXDEV_UNKNOWN    = 0xFF,
 } vtxDevType_e;

--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@ -89,6 +89,7 @@ bool cliMode = false;
 #include "io/beeper.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
+#include "io/gps_ublox.h"
 #include "io/ledstrip.h"
 #include "io/osd.h"
 #include "io/serial.h"
@ -3098,7 +3099,9 @@ static void cliSave(char *cmdline)

    cliPrint("Saving");
    //copyCurrentProfileToProfileSlot(getConfigProfile();
+    suspendRxSignal();
    writeEEPROM();
+    resumeRxSignal();
    cliReboot();
 }

@ -3108,6 +3111,9 @@ static void cliDefaults(char *cmdline)

    cliPrint("Resetting to defaults");
    resetEEPROM();
+    suspendRxSignal();
+    writeEEPROM();
+    resumeRxSignal();

 #ifdef USE_CLI_BATCH
    commandBatchError = false;
@ -3463,6 +3469,23 @@ static void cliStatus(char *cmdline)
    cliPrintLinefeed();
 #endif

+    if (featureConfigured(FEATURE_GPS) && (gpsConfig()->provider == GPS_UBLOX || gpsConfig()->provider == GPS_UBLOX7PLUS)) {
+        cliPrint("GPS: ");
+        cliPrintf("HW Version: %s Proto: %d.%02d Baud: %d", getGpsHwVersion(), getGpsProtoMajorVersion(), getGpsProtoMinorVersion(), getGpsBaudrate());
+        cliPrintLinefeed();
+        //cliPrintLinef("  GNSS Capabilities: %d", gpsUbloxCapLastUpdate());
+        cliPrintLinef("  GNSS Capabilities:");
+        cliPrintLine("    GNSS Provider active/default");
+        cliPrintLine("    GPS 1/1");
+        if(gpsUbloxHasGalileo())
+            cliPrintLinef("    Galileo %d/%d", gpsUbloxGalileoEnabled(), gpsUbloxGalileoDefault());
+        if(gpsUbloxHasBeidou())
+            cliPrintLinef("    BeiDou %d/%d", gpsUbloxBeidouEnabled(), gpsUbloxBeidouDefault());
+        if(gpsUbloxHasGlonass())
+            cliPrintLinef("    Glonass %d/%d", gpsUbloxGlonassEnabled(), gpsUbloxGlonassDefault());
+        cliPrintLinef("    Max concurrent: %d", gpsUbloxMaxGnss());
+    }
+
    // If we are blocked by PWM init - provide more information
    if (getPwmInitError() != PWM_INIT_ERROR_NONE) {
        cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage());
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@ -323,8 +323,6 @@ static void activateConfig(void)

 void readEEPROM(void)
 {
-    suspendRxSignal();
-
    // Sanity check, read flash
    if (!loadEEPROM()) {
        failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
@ -335,14 +333,14 @@ void readEEPROM(void)

    validateAndFixConfig();
    activateConfig();
-
-    resumeRxSignal();
 }

 void processSaveConfigAndNotify(void)
 {
+    suspendRxSignal();
    writeEEPROM();
    readEEPROM();
+    resumeRxSignal();
    beeperConfirmationBeeps(1);
 #ifdef USE_OSD
    osdShowEEPROMSavedNotification();
@ -351,15 +349,12 @@ void processSaveConfigAndNotify(void)

 void writeEEPROM(void)
 {
-    suspendRxSignal();
    writeConfigToEEPROM();
-    resumeRxSignal();
 }

 void resetEEPROM(void)
 {
    resetConfigs();
-    writeEEPROM();
 }

 void ensureEEPROMContainsValidData(void)
@ -368,6 +363,9 @@ void ensureEEPROMContainsValidData(void)
        return;
    }
    resetEEPROM();
+    suspendRxSignal();
+    writeEEPROM();
+    resumeRxSignal();
 }

 /*
@ -400,7 +398,9 @@ void processDelayedSave(void)
        processSaveConfigAndNotify();
        saveState = SAVESTATE_NONE;
    } else if (saveState == SAVESTATE_SAVEONLY) {
+        suspendRxSignal();
        writeEEPROM();
+        resumeRxSignal();
        saveState = SAVESTATE_NONE;
    }
 }
@ -430,8 +430,10 @@ void setConfigProfileAndWriteEEPROM(uint8_t profileIndex)
 {
    if (setConfigProfile(profileIndex)) {
        // profile has changed, so ensure current values saved before new profile is loaded
+        suspendRxSignal();
        writeEEPROM();
        readEEPROM();
+        resumeRxSignal();
    }
    beeperConfirmationBeeps(profileIndex + 1);
 }
@ -459,17 +461,19 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex)
 {
    if (setConfigBatteryProfile(profileIndex)) {
        // profile has changed, so ensure current values saved before new profile is loaded
+        suspendRxSignal();
        writeEEPROM();
        readEEPROM();
+        resumeRxSignal();
    }
    beeperConfirmationBeeps(profileIndex + 1);
 }

-void setGyroCalibration(int16_t getGyroZero[XYZ_AXIS_COUNT])
+void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT])
 {
-    gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X];
-    gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y];
-    gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z];
+    gyroConfigMutable()->gyro_zero_cal[X] = (int16_t) getGyroZero[X];
+    gyroConfigMutable()->gyro_zero_cal[Y] = (int16_t) getGyroZero[Y];
+    gyroConfigMutable()->gyro_zero_cal[Z] = (int16_t) getGyroZero[Z];
 }

 void setGravityCalibration(float getGravity)
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@ -138,7 +138,7 @@ uint8_t getConfigBatteryProfile(void);
 bool setConfigBatteryProfile(uint8_t profileIndex);
 void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex);

-void setGyroCalibration(int16_t getGyroZero[XYZ_AXIS_COUNT]);
+void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]);
 void setGravityCalibration(float getGravity);

 bool canSoftwareSerialBeUsed(void);
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@ -21,8 +21,6 @@

 #include "platform.h"

-FILE_COMPILE_FOR_SPEED
-
 #include "blackbox/blackbox.h"

 #include "build/debug.h"
@ -125,6 +123,21 @@ timeUs_t lastDisarmTimeUs = 0;
 static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
 static timeMs_t prearmActivationTime = 0;

+static bool isAccRequired(void) {
+    return isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
+        isModeActivationConditionPresent(BOXNAVRTH) ||
+        isModeActivationConditionPresent(BOXNAVWP) ||
+        isModeActivationConditionPresent(BOXANGLE) ||
+        isModeActivationConditionPresent(BOXHORIZON) ||
+        isModeActivationConditionPresent(BOXNAVALTHOLD) ||
+        isModeActivationConditionPresent(BOXHEADINGHOLD) ||
+        isModeActivationConditionPresent(BOXNAVLAUNCH) ||
+        isModeActivationConditionPresent(BOXTURNASSIST) ||
+        isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
+        isModeActivationConditionPresent(BOXSOARING) ||
+        failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_DROP_IT;
+}
+
 bool areSensorsCalibrating(void)
 {
 #ifdef USE_BARO
@ -145,11 +158,11 @@ bool areSensorsCalibrating(void)
    }
 #endif

-    if (!navIsCalibrationComplete()) {
+    if (!navIsCalibrationComplete() && isAccRequired()) {
        return true;
    }

-    if (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) {
+    if (!accIsCalibrationComplete() && sensors(SENSOR_ACC) && isAccRequired()) {
        return true;
    }

@ -267,21 +280,7 @@ static void updateArmingStatus(void)
            sensors(SENSOR_ACC) &&
            !STATE(ACCELEROMETER_CALIBRATED) &&
            // Require ACC calibration only if any of the setting might require it
-            (
-                isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
-                isModeActivationConditionPresent(BOXNAVRTH) ||
-                isModeActivationConditionPresent(BOXNAVWP) ||
-                isModeActivationConditionPresent(BOXANGLE) ||
-                isModeActivationConditionPresent(BOXHORIZON) ||
-                isModeActivationConditionPresent(BOXNAVALTHOLD) ||
-                isModeActivationConditionPresent(BOXHEADINGHOLD) ||
-                isModeActivationConditionPresent(BOXNAVLAUNCH) ||
-                isModeActivationConditionPresent(BOXTURNASSIST) ||
-                isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
-                isModeActivationConditionPresent(BOXSOARING) ||
-                failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_DROP_IT
-
-            )
+            isAccRequired()
        ) {
            ENABLE_ARMING_FLAG(ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED);
        }
@ -990,12 +989,12 @@ void taskUpdateRxMain(timeUs_t currentTimeUs)
 }

 // returns seconds
-float getFlightTime()
+float getFlightTime(void)
 {
    return US2S(flightTime);
 }

-float getArmTime()
+float getArmTime(void)
 {
    return US2S(armTime);
 }
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@ -119,6 +119,7 @@
 #include "io/vtx_control.h"
 #include "io/vtx_smartaudio.h"
 #include "io/vtx_tramp.h"
+#include "io/vtx_msp.h"
 #include "io/vtx_ffpv24g.h"
 #include "io/piniobox.h"

@ -174,7 +175,7 @@ void flashLedsAndBeep(void)
        LED1_TOGGLE;
        LED0_TOGGLE;
        delay(25);
-        if (!(getPreferredBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1))))
+        if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1))))
            BEEP_ON;
        delay(25);
        BEEP_OFF;
@ -223,7 +224,9 @@ void init(void)

    initEEPROM();
    ensureEEPROMContainsValidData();
+    suspendRxSignal();
    readEEPROM();
+    resumeRxSignal();

 #ifdef USE_UNDERCLOCK
    // Re-initialize system clock to their final values (if necessary)
@ -663,6 +666,10 @@ void init(void)
    vtxFuriousFPVInit();
 #endif

+#ifdef USE_VTX_MSP
+    vtxMspInit();
+#endif
+
 #endif // USE_VTX_CONTROL

    // Now that everything has powered up the voltage and cell count be determined.
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@ -834,7 +834,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
        sbufWriteU32(dst, getFlightTime()); // Flight time (seconds)

        // Throttle
-        sbufWriteU8(dst, getThrottlePercent()); // Throttle Percent
+        sbufWriteU8(dst, getThrottlePercent(true)); // Throttle Percent
        sbufWriteU8(dst, navigationIsControllingThrottle() ? 1 : 0); // Auto Throttle Flag (0 or 1)

        break;
@ -1289,7 +1289,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
        sbufWriteU16(dst, navConfig()->general.max_manual_speed);
        sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
        sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
-        sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold);
+        sbufWriteU8(dst, navConfig()->mc.althold_throttle_type);
        sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
        break;

@ -2255,7 +2255,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
            navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
            navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
            navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
-            navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src);
+            navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src);
            currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src);
        } else
            return MSP_RESULT_ERROR;
@ -2349,8 +2349,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)

    case MSP_RESET_CONF:
        if (!ARMING_FLAG(ARMED)) {
+            suspendRxSignal();
            resetEEPROM();
+            writeEEPROM();
            readEEPROM();
+            resumeRxSignal();
        } else
            return MSP_RESULT_ERROR;
        break;
@ -2380,8 +2383,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)

    case MSP_EEPROM_WRITE:
        if (!ARMING_FLAG(ARMED)) {
+            suspendRxSignal();
            writeEEPROM();
            readEEPROM();
+            resumeRxSignal();
        } else
            return MSP_RESULT_ERROR;
        break;
@ -2477,12 +2482,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                    if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) {  //value is band and channel
                        const uint8_t newBand = (newFrequency / 8) + 1;
                        const uint8_t newChannel = (newFrequency % 8) + 1;
+
+                        if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) {
+                            vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel);
+                        }
+
                        vtxSettingsConfigMutable()->band = newBand;
                        vtxSettingsConfigMutable()->channel = newChannel;
                    }

                    if (sbufBytesRemaining(src) > 1) {
-                        vtxSettingsConfigMutable()->power = sbufReadU8(src);
+                        uint8_t newPower = sbufReadU8(src);
+                        uint8_t currentPower = 0;
+                        vtxCommonGetPowerIndex(vtxDevice, &currentPower);
+                        if (newPower != currentPower) {
+                            vtxCommonSetPowerByIndex(vtxDevice, newPower);
+                            vtxSettingsConfigMutable()->power = newPower;
+                        }
+
                        // Delegate pitmode to vtx directly
                        const uint8_t newPitmode = sbufReadU8(src);
                        uint8_t currentPitmode = 0;
@ -2521,6 +2538,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
            gpsSol.flags.validVelNE = false;
            gpsSol.flags.validVelD = false;
            gpsSol.flags.validEPE = false;
+            gpsSol.flags.validTime = false;
            gpsSol.numSat = sbufReadU8(src);
            gpsSol.llh.lat = sbufReadU32(src);
            gpsSol.llh.lon = sbufReadU32(src);
@ -3478,7 +3496,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);

 #ifdef USE_BARO
+            if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
 				baroStartCalibration();
+            }
 #endif
 #ifdef USE_MAG
 				DISABLE_STATE(COMPASS_CALIBRATED);
@ -3489,10 +3509,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu

 				disarm(DISARM_SWITCH);  // Disarm to prevent motor output!!!
 			}
-		} else if (!areSensorsCalibrating()) {
+        } else {
 			if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
 #ifdef USE_BARO
-				baroStartCalibration();
+                if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
+                    sensorsSet(SENSOR_BARO);
+                    setTaskEnabled(TASK_BARO, true);
+                    DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
+				    baroStartCalibration();
+                }
 #endif

 #ifdef USE_MAG
@ -3521,6 +3546,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 						gpsSol.flags.validVelNE = true;
 						gpsSol.flags.validVelD = true;
 						gpsSol.flags.validEPE = true;
+						gpsSol.flags.validTime = false;

 						gpsSol.llh.lat = sbufReadU32(src);
 						gpsSol.llh.lon = sbufReadU32(src);
@ -3575,7 +3601,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu

 				if (sensors(SENSOR_MAG)) {
 					mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20;  // 16000 / 20 = 800uT
-					mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;
+					mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;   //note that mag failure is simulated by setting all readings to zero
 					mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
 				} else {
 					sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
@ -3593,7 +3619,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu

                if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
                    simulatorData.airSpeed = sbufReadU16(src);
-			    }
+			    } else {
+                    if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
+                        sbufReadU16(src);
+                    }
+                }
+
+                if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
+                    simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8;
+                }
 			} else {
 				DISABLE_STATE(GPS_FIX);
 			}
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@ -64,6 +64,7 @@
 #include "io/rcdevice_cam.h"
 #include "io/smartport_master.h"
 #include "io/vtx.h"
+#include "io/vtx_msp.h"
 #include "io/osd_dji_hd.h"
 #include "io/displayport_msp_osd.h"
 #include "io/servo_sbus.h"
@ -109,6 +110,9 @@ void taskHandleSerial(timeUs_t currentTimeUs)
 #ifdef USE_MSP_OSD
 	// Capture MSP Displayport messages to determine if VTX is connected
    mspOsdSerialProcess(mspFcProcessCommand);
+#ifdef USE_VTX_MSP
+    mspVtxSerialProcess(mspFcProcessCommand);
+#endif
 #endif

 }
--- a/src/main/fc/runtime_config.c
+++ b/src/main/fc/runtime_config.c
@ -179,7 +179,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)

 #ifdef USE_SIMULATOR
 simulatorData_t simulatorData = {
-	flags: 0,
-	debugIndex: 0
+	.flags = 0,
+	.debugIndex = 0
 };
 #endif
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@ -175,20 +175,23 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void);

 typedef enum {
    HITL_RESET_FLAGS            = (0 << 0),
-	HITL_ENABLE					= (1 << 0),
-	HITL_SIMULATE_BATTERY		= (1 << 1),
-	HITL_MUTE_BEEPER			= (1 << 2),
-	HITL_USE_IMU			    = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV)
-	HITL_HAS_NEW_GPS_DATA		= (1 << 4),
-	HITL_EXT_BATTERY_VOLTAGE	= (1 << 5), // Extend MSP_SIMULATOR format 2
-    HITL_AIRSPEED               = (1 << 6) 
+    HITL_ENABLE					= (1 << 0),
+    HITL_SIMULATE_BATTERY		= (1 << 1),
+    HITL_MUTE_BEEPER			= (1 << 2),
+    HITL_USE_IMU			    = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV)
+    HITL_HAS_NEW_GPS_DATA		= (1 << 4),
+    HITL_EXT_BATTERY_VOLTAGE	= (1 << 5), // Extend MSP_SIMULATOR format 2
+    HITL_AIRSPEED               = (1 << 6),
+    HITL_EXTENDED_FLAGS         = (1 << 7), // Extend MSP_SIMULATOR format 2
+    HITL_GPS_TIMEOUT            = (1 << 8),
+    HITL_PITOT_FAILURE          = (1 << 9)
 } simulatorFlags_t;

 typedef struct {
-	simulatorFlags_t flags;
-	uint8_t debugIndex;
+    simulatorFlags_t flags;
+    uint8_t debugIndex;
    uint8_t vbat;      // 126 -> 12.6V
-	uint16_t airSpeed; // cm/s
+    uint16_t airSpeed; // cm/s
    int16_t input[4];
 } simulatorData_t;

--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@ -25,7 +25,7 @@ tables:
    values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
    enum: rxReceiverType_e
  - name: serial_rx
-    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK"]
+    values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK", "FBUS"]
  - name: blackbox_device
    values: ["SERIAL", "SPIFLASH", "SDCARD"]
  - name: motor_pwm_protocol
@ -190,6 +190,12 @@ tables:
  - name: nav_fw_wp_turn_smoothing
    values: ["OFF", "ON", "ON-CUT"]
    enum: wpFwTurnSmoothing_e
+  - name: gps_auto_baud_max
+    values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600']
+    enum: gpsBaudRate_e
+  - name: nav_mc_althold_throttle
+    values: ["STICK", "MID_STICK", "HOVER"]
+    enum: navMcAltHoldThrottle_e    

 constants:
  RPYL_PID_MIN: 0
@ -1501,6 +1507,7 @@ groups:
        min: 1
        max: 3000
  - name: PG_GPS_CONFIG
+    headers: [ "io/gps.h" ]
    type: gpsConfig_t
    condition: USE_GPS
    members:
@ -1532,17 +1539,41 @@ groups:
        default_value: ON
        field: autoBaud
        type: bool
+      - name: gps_auto_baud_max_supported
+        description: "Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0"
+        default_value: "230400"
+        table: gps_auto_baud_max
+        field: autoBaudMax
+        type: uint8_t
      - name: gps_ublox_use_galileo
        description: "Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]."
        default_value: OFF
        field: ubloxUseGalileo
        type: bool
+      - name: gps_ublox_use_beidou
+        description: "Enable use of Beidou satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps hardware support [OFF/ON]."
+        default_value: OFF
+        field: ubloxUseBeidou
+        type: bool
+      - name: gps_ublox_use_glonass
+        description: "Enable use of Glonass satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires gps haardware support [OFF/ON]."
+        default_value: OFF
+        field: ubloxUseGlonass
+        type: bool
      - name: gps_min_sats
        description: "Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count."
        default_value: 6
        field: gpsMinSats
        min: 5
        max: 10
+      - name: gps_ublox_nav_hz
+        description: "Navigation update rate for UBLOX7 receivers. Some receivers may limit the maximum number of satellites tracked when set to a higher rate or even stop sending navigation updates if the value is too high. Some M10 devices can do up to 25Hz. 10 is a safe value for M8 and newer."
+        default_value: 10
+        field: ubloxNavHz
+        type: uint8_t
+        min: 5
+        max: 200
+

  - name: PG_RC_CONTROLS_CONFIG
    type: rcControlsConfig_t
@ -2287,11 +2318,16 @@ groups:
        field: general.land_detect_sensitivity
        min: 1
        max: 15
-      - name: nav_use_midthr_for_althold
-        description: "If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude"
+      - name: nav_landing_bump_detection
+        description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors."
        default_value: OFF
-        field: general.flags.use_thr_mid_for_althold
+        field: general.flags.landing_bump_detection
        type: bool
+      - name: nav_mc_althold_throttle
+        description: "If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively."
+        default_value: "STICK"
+        field: mc.althold_throttle_type
+        table: nav_mc_althold_throttle
      - name: nav_extra_arming_safety
        description: "If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used"
        default_value: "ALLOW_BYPASS"
--- a/src/main/fc/stats.c
+++ b/src/main/fc/stats.c
@ -36,7 +36,7 @@ static uint32_t arm_distance_cm;
 static uint32_t arm_mWhDrawn;
 static uint32_t flyingEnergy; // energy drawn during flying up to last disarm (ARMED) mWh

-uint32_t getFlyingEnergy() {
+uint32_t getFlyingEnergy(void) {
    return flyingEnergy;
 }
 #endif
--- a/src/main/flight/dynamic_gyro_notch.c
+++ b/src/main/flight/dynamic_gyro_notch.c
@ -26,8 +26,6 @@

 #ifdef USE_DYNAMIC_FILTERS

-FILE_COMPILE_FOR_SPEED
-
 #include <stdint.h>
 #include "dynamic_gyro_notch.h"
 #include "fc/config.h"
--- a/src/main/flight/dynamic_lpf.c
+++ b/src/main/flight/dynamic_lpf.c
@ -41,7 +41,8 @@ void dynamicLpfGyroTask(void) {
        return;
    }

-    const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
+    const float throttleConstrained = (float) constrain(rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
+    const float throttle = scaleRangef(throttleConstrained, getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
    const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);

    DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq);
--- a/src/main/flight/gyroanalyse.c
+++ b/src/main/flight/gyroanalyse.c
@ -26,7 +26,6 @@
 #include <stdint.h>

 #include "platform.h"
-FILE_COMPILE_FOR_SPEED

 #ifdef USE_DYNAMIC_FILTERS

--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@ -23,8 +23,6 @@

 #include "platform.h"

-FILE_COMPILE_FOR_SPEED
-
 #include "blackbox/blackbox.h"

 #include "build/build_config.h"
@ -279,7 +277,7 @@ static void imuResetOrientationQuaternion(const fpVector3_t * accBF)

 static bool imuValidateQuaternion(const fpQuaternion_t * quat)
 {
-    const float check = fabs(quat->q0) + fabs(quat->q1) + fabs(quat->q2) + fabs(quat->q3);
+    const float check = fabsf(quat->q0) + fabsf(quat->q1) + fabsf(quat->q2) + fabsf(quat->q3);

    if (!isnan(check) && !isinf(check)) {
        return true;
@ -482,7 +480,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
    const float thetaMagnitudeSq = vectorNormSquared(&vTheta);

    // If calculated rotation is zero - don't update quaternion
-    if (thetaMagnitudeSq >= 1e-20) {
+    if (thetaMagnitudeSq >= 1e-20f) {
        // Calculate quaternion delta:
        // Theta is a axis/angle rotation. Direction of a vector is axis, magnitude is angle/2.
        // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.
--- a/src/main/flight/kalman.c
+++ b/src/main/flight/kalman.c
@ -20,8 +20,6 @@
 #include "platform.h"
 #ifdef USE_GYRO_KALMAN

-FILE_COMPILE_FOR_SPEED
-
 #include <string.h>
 #if !defined(SITL_BUILD)
 #include "arm_math.h"
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@ -21,8 +21,6 @@

 #include "platform.h"

-FILE_COMPILE_FOR_SPEED
-
 #include "build/debug.h"

 #include "common/axis.h"
@ -463,7 +461,7 @@ static int getReversibleMotorsThrottleDeadband(void)
    return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue;
 }

-void FAST_CODE mixTable()
+void FAST_CODE mixTable(void)
 {
 #ifdef USE_DSHOT
    if (FLIGHT_MODE(TURTLE_MODE)) {
@ -614,9 +612,16 @@ void FAST_CODE mixTable()
    }
 }

-int16_t getThrottlePercent(void)
+int16_t getThrottlePercent(bool useScaled)
 {
-    int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - getThrottleIdleValue()) * 100 / (motorConfig()->maxthrottle - getThrottleIdleValue());
+    int16_t thr = constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
+    const int idleThrottle = getThrottleIdleValue();
+    
+    if (useScaled) {
+       thr = (thr - idleThrottle) * 100 / (motorConfig()->maxthrottle - idleThrottle);
+    } else {
+        thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
+    }
    return thr;
 }

--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@ -111,7 +111,7 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 extern int mixerThrottleCommand;

 int getThrottleIdleValue(void);
-int16_t getThrottlePercent(void);
+int16_t getThrottlePercent(bool);
 uint8_t getMotorCount(void);
 float getMotorMixRange(void);
 bool mixerIsOutputSaturated(void);
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@ -21,8 +21,6 @@

 #include <platform.h>

-FILE_COMPILE_FOR_SPEED
-
 #include "build/build_config.h"
 #include "build/debug.h"

@ -156,7 +154,7 @@ static EXTENDED_FASTRAM float iTermAntigravityGain;
 #endif
 static EXTENDED_FASTRAM uint8_t usedPidControllerType;

-typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
+typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv);
 static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
@ -307,7 +305,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 #endif
 );

-FUNCTION_COMPILE_FOR_SIZE
 bool pidInitFilters(void)
 {
    const uint32_t refreshRate = getLooptime();
@ -480,7 +477,7 @@ void schedulePidGainsUpdate(void)
    pidGainsUpdateRequired = true;
 }

-void updatePIDCoefficients()
+void updatePIDCoefficients(void)
 {
    STATIC_FASTRAM uint16_t prevThrottle = 0;

@ -628,7 +625,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam
        }
    }

-    float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
+    float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);

    // Apply simple LPF to angleRateTarget to make response less jerky
    // Ideas behind this:
@ -684,13 +681,13 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
 }

 #ifdef USE_D_BOOST
-static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT) {
+static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {

    float dBoost = 1.0f;

-    const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT;
+    const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv;
    const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
-    const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) / dT);
+    const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv);

    if (dBoostGyroAcceleration >= dBoostRateAcceleration) {
        //Gyro is accelerating faster than setpoint, we want to smooth out
@ -713,7 +710,7 @@ static float applyDBoost(pidState_t *pidState, float dT) {
 }
 #endif

-static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT) {
+static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
    // Calculate new D-term
    float newDTerm = 0;
    if (pidState->kD == 0) {
@ -725,7 +722,7 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d
        delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);

        // Calculate derivative
-        newDTerm =  delta * (pidState->kD / dT) * applyDBoost(pidState, currentRateTarget, dT);
+        newDTerm =  delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
    }
    return(newDTerm);
 }
@ -739,19 +736,20 @@ static void applyItermLimiting(pidState_t *pidState) {
    }
 }

-static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
+static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) {
    UNUSED(pidState);
    UNUSED(axis);
    UNUSED(dT);
+    UNUSED(dT_inv);
 }

-static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
+static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
 {
    const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);

    const float rateError = rateTarget - pidState->gyroRate;
    const float newPTerm = pTermProcess(pidState, rateError, dT);
-    const float newDTerm = dTermProcess(pidState, rateTarget, dT);
+    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
    const float newFFTerm = rateTarget * pidState->kFF;

    /*
@ -809,14 +807,14 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
    return itermErrorRate;
 }

-static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
+static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
 {

    const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);

    const float rateError = rateTarget - pidState->gyroRate;
    const float newPTerm = pTermProcess(pidState, rateError, dT);
-    const float newDTerm = dTermProcess(pidState, rateTarget, dT);
+    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);

    const float rateTargetDelta = rateTarget - pidState->previousRateTarget;
    const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta);
@ -867,7 +865,7 @@ void resetHeadingHoldTarget(int16_t heading)
    pt1FilterReset(&headingHoldRateFilter, 0.0f);
 }

-int16_t getHeadingHoldTarget() {
+int16_t getHeadingHoldTarget(void) {
    return headingHoldTarget;
 }

@ -888,8 +886,6 @@ static uint8_t getHeadingHoldState(void)
    }
    else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) {
        return HEADING_HOLD_ENABLED;
-    } else {
-        return HEADING_HOLD_UPDATE_HEADING;
    }

    return HEADING_HOLD_UPDATE_HEADING;
@ -1065,6 +1061,9 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis

 void FAST_CODE pidController(float dT)
 {
+
+    const float dT_inv = 1.0f / dT;
+
    if (!pidFiltersConfigured) {
        return;
    }
@ -1126,7 +1125,7 @@ void FAST_CODE pidController(float dT)
        }
    }

-    if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
+    if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
        float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
        float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
        pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
@ -1138,7 +1137,7 @@ void FAST_CODE pidController(float dT)
    }

    // Prevent strong Iterm accumulation during stick inputs
-    antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
+    antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f);

    for (int axis = 0; axis < 3; axis++) {
        // Apply setpoint rate of change limits
@ -1148,7 +1147,7 @@ void FAST_CODE pidController(float dT)
        checkItermLimitingActive(&pidState[axis]);
        checkItermFreezingActive(&pidState[axis], axis);

-        pidControllerApplyFn(&pidState[axis], axis, dT);
+        pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv);
    }
 }

@ -1180,7 +1179,7 @@ void pidInit(void)
    itermRelax = pidProfile()->iterm_relax;

    yawLpfHz = pidProfile()->yaw_lpf_hz;
-    motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
+    motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f));

 #ifdef USE_D_BOOST
    dBoostMin = pidProfile()->dBoostMin;
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@ -47,7 +47,7 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
 #define FP_PID_RATE_I_MULTIPLIER    4.0f
 #define FP_PID_RATE_D_MULTIPLIER    1905.0f
 #define FP_PID_RATE_D_FF_MULTIPLIER   7270.0f
-#define FP_PID_LEVEL_P_MULTIPLIER   6.56f       // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
+#define FP_PID_LEVEL_P_MULTIPLIER   1.0f / 6.56f       // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
 #define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f

 #define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
--- a/src/main/flight/pid_autotune.c
+++ b/src/main/flight/pid_autotune.c
@ -187,7 +187,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa

    // Use different max rate in ANLGE mode
    if (FLIGHT_MODE(ANGLE_MODE)) {
-        float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
+        float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER;
        maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode);
    }

--- a/src/main/flight/power_limits.c
+++ b/src/main/flight/power_limits.c
@ -20,7 +20,6 @@
 #include <math.h>

 #include "platform.h"
-FILE_COMPILE_FOR_SPEED

 #if defined(USE_POWER_LIMITS)

@ -163,14 +162,14 @@ void powerLimiterApply(int16_t *throttleCommand) {
    int32_t overCurrent = current - activeCurrentLimit;

    if (lastCallTimestamp) {
-        currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
+        currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7f, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
    }

-    float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3;
+    float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3f;

-    uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6));
+    uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6f));

-    throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
+    throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6f)) : *throttleCommand;
    uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn);

    if (activeCurrentLimit && currentThrAttned < *throttleCommand) {
@ -192,12 +191,12 @@ void powerLimiterApply(int16_t *throttleCommand) {
    int32_t overPower = power - activePowerLimit;

    if (lastCallTimestamp) {
-        powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
+        powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5f, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
    }

-    float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1;
+    float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1f;

-    uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6));
+    uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6f));

    throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
    uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn);
@ -245,11 +244,11 @@ bool powerLimiterIsLimitingPower(void) {
 // returns seconds
 float powerLimiterGetRemainingBurstTime(void) {
    uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
-    float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7;
+    float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7f;

 #ifdef USE_ADC
    uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
-    float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7;
+    float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7f;

    if (!currentBatteryProfile->powerLimits.continuousCurrent) {
        return remainingPowerBurstTime;
--- a/Show more
+++ b/Show more