diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index d811dc242ac..2f9cd7c287f 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -75,7 +75,7 @@ jobs:
       - uses: actions/checkout@v3
 
       - name: Install dependencies
-        run: sudo apt-get install -y libblocksruntime-dev
+        run: sudo apt-get install -y libblocksruntime-dev clang-12
 
       - name: Run sanity checks
         run: make EXTRA_FLAGS=-Werror checks
diff --git a/Makefile b/Makefile
index 1dde7cc6efb..fdf87781eb5 100644
--- a/Makefile
+++ b/Makefile
@@ -16,8 +16,8 @@
 #
 
 # The target to build, see VALID_TARGETS below
-TARGET    ?= STM32F405
-BOARD     ?= 
+DEFAULT_TARGET    ?= STM32F405
+TARGET    ?=
 
 # Compile-time options
 OPTIONS   ?=
@@ -80,8 +80,8 @@ include $(ROOT)/make/system-id.mk
 include $(ROOT)/make/checks.mk
 
 # configure some directories that are relative to wherever ROOT_DIR is located
-TOOLS_DIR ?= $(ROOT)/tools
-DL_DIR    := $(ROOT)/downloads
+TOOLS_DIR  ?= $(ROOT)/tools
+DL_DIR     := $(ROOT)/downloads
 
 export RM := rm
 
@@ -97,9 +97,8 @@ HSE_VALUE       ?= 8000000
 # used for turning on features like VCP and SDCARD
 FEATURES        =
 
-ifneq ($(BOARD),)
-# silently ignore if the file is not present. Allows for target defaults.
--include $(ROOT)/src/main/board/$(BOARD)/board.mk
+ifeq ($(TARGET),)
+TARGET := $(DEFAULT_TARGET)
 endif
 
 include $(ROOT)/make/targets.mk
@@ -283,28 +282,34 @@ CPPCHECK        = cppcheck $(CSOURCES) --enable=all --platform=unix64 \
                   $(addprefix -I,$(INCLUDE_DIRS)) \
                   -I/usr/include -I/usr/include/linux
 
-TARGET_BASENAME = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET)
+TARGET_NAME := $(TARGET)
+
+ifeq ($(REV),yes)
+TARGET_NAME := $(TARGET_NAME)_$(REVISION)
+endif
+
+TARGET_FULLNAME = $(FORKNAME)_$(FC_VER)_$(TARGET_NAME)
 
 #
 # Things we will build
 #
-TARGET_BIN      = $(TARGET_BASENAME).bin
-TARGET_HEX      = $(TARGET_BASENAME).hex
-TARGET_HEX_REV  = $(TARGET_BASENAME)_$(REVISION).hex
-TARGET_DFU      = $(TARGET_BASENAME).dfu
-TARGET_ZIP      = $(TARGET_BASENAME).zip
-TARGET_ELF      = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf
-TARGET_EXST_ELF = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET)_EXST.elf
-TARGET_UNPATCHED_BIN = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET)_UNPATCHED.bin
-TARGET_LST      = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).lst
-TARGET_OBJS     = $(addsuffix .o,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $(SRC))))
-TARGET_DEPS     = $(addsuffix .d,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $(SRC))))
-TARGET_MAP      = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map
-
-TARGET_EXST_HASH_SECTION_FILE = $(OBJECT_DIR)/$(TARGET)/exst_hash_section.bin
+TARGET_BIN      = $(BIN_DIR)/$(TARGET_FULLNAME).bin
+TARGET_HEX      = $(BIN_DIR)/$(TARGET_FULLNAME).hex
+TARGET_DFU      = $(BIN_DIR)/$(TARGET_FULLNAME).dfu
+TARGET_ZIP      = $(BIN_DIR)/$(TARGET_FULLNAME).zip
+TARGET_OBJ_DIR  = $(OBJECT_DIR)/$(TARGET_NAME)
+TARGET_ELF      = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET_NAME).elf
+TARGET_EXST_ELF = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET_NAME)_EXST.elf
+TARGET_UNPATCHED_BIN = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET_NAME)_UNPATCHED.bin
+TARGET_LST      = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET_NAME).lst
+TARGET_OBJS     = $(addsuffix .o,$(addprefix $(TARGET_OBJ_DIR)/,$(basename $(SRC))))
+TARGET_DEPS     = $(addsuffix .d,$(addprefix $(TARGET_OBJ_DIR)/,$(basename $(SRC))))
+TARGET_MAP      = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET_NAME).map
+
+TARGET_EXST_HASH_SECTION_FILE = $(TARGET_OBJ_DIR)/exst_hash_section.bin
 
 TARGET_EF_HASH      := $(shell echo -n "$(EXTRA_FLAGS)" | openssl dgst -md5 | awk '{print $$2;}')
-TARGET_EF_HASH_FILE := $(OBJECT_DIR)/$(TARGET)/.efhash_$(TARGET_EF_HASH)
+TARGET_EF_HASH_FILE := $(TARGET_OBJ_DIR)/.efhash_$(TARGET_EF_HASH)
 
 CLEAN_ARTIFACTS := $(TARGET_BIN)
 CLEAN_ARTIFACTS += $(TARGET_HEX_REV) $(TARGET_HEX)
@@ -313,7 +318,7 @@ CLEAN_ARTIFACTS += $(TARGET_LST)
 CLEAN_ARTIFACTS += $(TARGET_DFU)
 
 # Make sure build date and revision is updated on every incremental build
-$(OBJECT_DIR)/$(TARGET)/build/version.o : $(SRC)
+$(TARGET_OBJ_DIR)/build/version.o : $(SRC)
 
 # List of buildable ELF files and their object dependencies.
 # It would be nice to compute these lists, but that seems to be just beyond make.
@@ -367,11 +372,8 @@ $(TARGET_BIN): $(TARGET_UNPATCHED_BIN)
 	@echo "Patching MD5 hash into HASH section" "$(STDOUT)"
 	$(V1) cat $(TARGET_UNPATCHED_BIN).md5 | awk '{printf("%08x: %s",64-16,$$2);}' | xxd -r - $(TARGET_EXST_HASH_SECTION_FILE)
 
-# For some currently unknown reason, OBJCOPY, with only input/output files, will generate a file around 2GB for the H730 unless we remove an unused-section
-# As a workaround drop the ._user_heap_stack section, which is only used during build to show errors if there's not enough space for the heap/stack. 
-# The issue can be seen with `readelf -S $(TARGET_EXST_ELF)' vs `readelf -S $(TARGET_ELF)`
 	$(V1) @echo "Patching updated HASH section into $(TARGET_EXST_ELF)" "$(STDOUT)"
-	$(OBJCOPY) $(TARGET_ELF) $(TARGET_EXST_ELF) --remove-section ._user_heap_stack --update-section .exst_hash=$(TARGET_EXST_HASH_SECTION_FILE)
+	$(OBJCOPY) $(TARGET_ELF) $(TARGET_EXST_ELF) --update-section .exst_hash=$(TARGET_EXST_HASH_SECTION_FILE)
 
 	$(V1) $(READELF) -S $(TARGET_EXST_ELF)
 	$(V1) $(READELF) -l $(TARGET_EXST_ELF)
@@ -385,7 +387,7 @@ $(TARGET_HEX): $(TARGET_BIN)
 endif
 
 $(TARGET_ELF): $(TARGET_OBJS) $(LD_SCRIPT) $(LD_SCRIPTS)
-	@echo "Linking $(TARGET)" "$(STDOUT)"
+	@echo "Linking $(TARGET_NAME)" "$(STDOUT)"
 	$(V1) $(CROSS_CC) -o $@ $(filter-out %.ld,$^) $(LD_FLAGS)
 	$(V1) $(SIZE) $(TARGET_ELF)
 
@@ -398,7 +400,7 @@ define compile_file
 endef
 
 ifeq ($(DEBUG),GDB)
-$(OBJECT_DIR)/$(TARGET)/%.o: %.c
+$(TARGET_OBJ_DIR)/%.o: %.c
 	$(V1) mkdir -p $(dir $@)
 	$(V1) $(if $(findstring $<,$(NOT_OPTIMISED_SRC)), \
 		$(call compile_file,not optimised, $(CC_NO_OPTIMISATION)) \
@@ -406,7 +408,7 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.c
 		$(call compile_file,debug,$(CC_DEBUG_OPTIMISATION)) \
 	)
 else
-$(OBJECT_DIR)/$(TARGET)/%.o: %.c
+$(TARGET_OBJ_DIR)/%.o: %.c
 	$(V1) mkdir -p $(dir $@)
 	$(V1) $(if $(findstring $<,$(NOT_OPTIMISED_SRC)), \
 		$(call compile_file,not optimised,$(CC_NO_OPTIMISATION)) \
@@ -424,12 +426,12 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.c
 endif
 
 # Assemble
-$(OBJECT_DIR)/$(TARGET)/%.o: %.s
+$(TARGET_OBJ_DIR)/%.o: %.s
 	$(V1) mkdir -p $(dir $@)
 	@echo "%% $(notdir $<)" "$(STDOUT)"
 	$(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $<
 
-$(OBJECT_DIR)/$(TARGET)/%.o: %.S
+$(TARGET_OBJ_DIR)/%.o: %.S
 	$(V1) mkdir -p $(dir $@)
 	@echo "%% $(notdir $<)" "$(STDOUT)"
 	$(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $<
@@ -441,7 +443,7 @@ all: $(CI_TARGETS)
 ## all_all : Build all targets (including legacy / unsupported)
 all_all: $(VALID_TARGETS)
 
-$(VALID_TARGETS):
+$(BASE_TARGETS):
 	$(V0) @echo "Building $@" && \
 	$(MAKE) hex TARGET=$@ && \
 	echo "Building $@ succeeded."
@@ -453,10 +455,10 @@ TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS))
 
 ## clean             : clean up temporary / machine-generated files
 clean:
-	@echo "Cleaning $(TARGET)"
+	@echo "Cleaning $(TARGET_NAME)"
 	$(V0) rm -f $(CLEAN_ARTIFACTS)
-	$(V0) rm -rf $(OBJECT_DIR)/$(TARGET)
-	@echo "Cleaning $(TARGET) succeeded."
+	$(V0) rm -rf $(TARGET_OBJ_DIR)
+	@echo "Cleaning $(TARGET_NAME) succeeded."
 
 ## test_clean        : clean up temporary / machine-generated files (tests)
 test-%_clean:
@@ -529,10 +531,7 @@ hex:
 TARGETS_REVISION = $(addsuffix _rev,$(VALID_TARGETS))
 ## <TARGET>_rev    : build target and add revision to filename
 $(TARGETS_REVISION):
-	$(V0) $(MAKE) hex_rev TARGET=$(subst _rev,,$@)
-
-hex_rev: hex
-	$(V0) mv -f $(TARGET_HEX) $(TARGET_HEX_REV)
+	$(V0) $(MAKE) hex REV=yes TARGET=$(subst _rev,,$@)
 
 all_rev: $(addsuffix _rev,$(CI_TARGETS))
 
@@ -648,7 +647,7 @@ test_%:
 
 $(TARGET_EF_HASH_FILE):
 	$(V1) mkdir -p $(dir $@)
-	$(V0) rm -f $(OBJECT_DIR)/$(TARGET)/.efhash_*
+	$(V0) rm -f $(TARGET_OBJ_DIR)/.efhash_*
 	@echo "EF HASH -> $(TARGET_EF_HASH_FILE)"
 	$(V1) touch $(TARGET_EF_HASH_FILE)
 
diff --git a/make/mcu/STM32H7.mk b/make/mcu/STM32H7.mk
index 5649818ecb6..69f5fd2cc1b 100644
--- a/make/mcu/STM32H7.mk
+++ b/make/mcu/STM32H7.mk
@@ -207,9 +207,22 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(H723xG_TARGETS)))
 DEVICE_FLAGS       += -DSTM32H723xx
 DEFAULT_LD_SCRIPT   = $(LINKER_DIR)/stm32_flash_h723_1m.ld
 STARTUP_SRC         = startup_stm32h723xx.s
-MCU_FLASH_SIZE     := 1024
+DEFAULT_TARGET_FLASH := 1024
 DEVICE_FLAGS       += -DMAX_MPU_REGIONS=16
 
+ifeq ($(TARGET_FLASH),)
+MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH)
+endif
+
+ifeq ($(EXST),yes)
+FIRMWARE_SIZE      := 1024
+# TARGET_FLASH now becomes the amount of MEMORY-MAPPED address space that is occupied by the firmware
+# and the maximum size of the data stored on the external flash device.
+MCU_FLASH_SIZE     := FIRMWARE_SIZE
+DEFAULT_LD_SCRIPT   = $(LINKER_DIR)/stm32_ram_h723_exst.ld
+LD_SCRIPTS          = $(LINKER_DIR)/stm32_h723_common.ld $(LINKER_DIR)/stm32_h723_common_post.ld
+endif
+
 else ifeq ($(TARGET),$(filter $(TARGET),$(H725xG_TARGETS)))
 DEVICE_FLAGS       += -DSTM32H725xx
 DEFAULT_LD_SCRIPT   = $(LINKER_DIR)/stm32_flash_h723_1m.ld
@@ -226,7 +239,7 @@ DEVICE_FLAGS       += -DMAX_MPU_REGIONS=16
 
 
 ifeq ($(TARGET_FLASH),)
-MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH) 
+MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH)
 endif
 
 ifeq ($(EXST),yes)
@@ -246,7 +259,7 @@ STARTUP_SRC         = startup_stm32h743xx.s
 DEFAULT_TARGET_FLASH := 128
 
 ifeq ($(TARGET_FLASH),)
-MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH) 
+MCU_FLASH_SIZE := $(DEFAULT_TARGET_FLASH)
 endif
 
 ifeq ($(EXST),yes)
@@ -281,7 +294,7 @@ LD_SCRIPT = $(DEFAULT_LD_SCRIPT)
 endif
 
 ifneq ($(FIRMWARE_SIZE),)
-DEVICE_FLAGS   += -DFIRMWARE_SIZE=$(FIRMWARE_SIZE) 
+DEVICE_FLAGS   += -DFIRMWARE_SIZE=$(FIRMWARE_SIZE)
 endif
 
 DEVICE_FLAGS    += -DHSE_VALUE=$(HSE_VALUE) -DHSE_STARTUP_TIMEOUT=1000
@@ -304,6 +317,7 @@ MCU_COMMON_SRC = \
             drivers/serial_uart_hal.c \
             drivers/serial_uart_stm32h7xx.c \
             drivers/bus_quadspi_hal.c \
+            drivers/bus_octospi_stm32h7xx.c \
             drivers/bus_spi_ll.c \
             drivers/dma_stm32h7xx.c \
             drivers/dshot_bitbang.c \
diff --git a/make/source.mk b/make/source.mk
index 1590e1b4860..e2c897cb259 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -20,6 +20,7 @@ COMMON_SRC = \
             drivers/bus_i2c_config.c \
             drivers/bus_i2c_busdev.c \
             drivers/bus_i2c_soft.c \
+            drivers/bus_octospi.c \
             drivers/bus_quadspi.c \
             drivers/bus_spi.c \
             drivers/bus_spi_config.c \
@@ -450,12 +451,6 @@ SRC += $(MSC_SRC)
 endif
 # end target specific make file checks
 
-ifneq ($(BOARD),)
-SRC += board/$(BOARD)/board.c
-INCLUDE_DIRS += $(ROOT)/src/main/board/$(BOARD)
-TARGET_FLAGS := -D'__BOARD__="$(BOARD)"' $(TARGET_FLAGS)
-endif
-
 # Search path and source files for the ST stdperiph library
 VPATH        := $(VPATH):$(STDPERIPH_DIR)/src
 
diff --git a/src/link/stm32_flash_g4_split.ld b/src/link/stm32_flash_g4_split.ld
index 7820c38964a..843d2739356 100644
--- a/src/link/stm32_flash_g4_split.ld
+++ b/src/link/stm32_flash_g4_split.ld
@@ -39,7 +39,7 @@ SECTIONS
 
   .isr_vector :
   {
-    . = ALIGN(512);
+    . = ALIGN(4);
     PROVIDE (isr_vector_table_base = .);
     KEEP(*(.isr_vector)) /* Startup code */
     . = ALIGN(4);
diff --git a/src/link/stm32_h723_common.ld b/src/link/stm32_h723_common.ld
new file mode 100644
index 00000000000..d10f3d957fa
--- /dev/null
+++ b/src/link/stm32_h723_common.ld
@@ -0,0 +1,187 @@
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/* Highest address of the user mode stack */
+_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM);    /* end of RAM */
+
+/* Base address where the quad spi. */
+__octospi1_start = ORIGIN(OCTOSPI1);
+__octospi2_start = ORIGIN(OCTOSPI2);
+
+/* Generate a link error if heap and stack don't fit into RAM */
+_Min_Heap_Size = 0;      /* required amount of heap  */
+_Min_Stack_Size = 0x800; /* required amount of stack */
+
+/* Define output sections */
+SECTIONS
+{
+  _isr_vector_table_flash_base = LOADADDR(.isr_vector);
+  PROVIDE (isr_vector_table_flash_base = _isr_vector_table_flash_base);
+
+  .isr_vector :
+  {
+    . = ALIGN(512);
+    PROVIDE (isr_vector_table_base = .);
+    KEEP(*(.isr_vector)) /* Startup code */
+    . = ALIGN(4);
+    PROVIDE (isr_vector_table_end = .);
+  } >VECTAB AT> MAIN
+
+  _ram_isr_vector_table_base = LOADADDR(.ram_isr_vector);
+  PROVIDE (ram_isr_vector_table_base = _ram_isr_vector_table_base);
+
+  .ram_isr_vector (NOLOAD) :
+  {
+    . = ALIGN(512); /* Vector table offset must be multiple of 0x200 */
+    PROVIDE (ram_isr_vector_table_base = .);
+    . += (isr_vector_table_end - isr_vector_table_base);
+    . = ALIGN(4);    
+    PROVIDE (ram_isr_vector_table_end = .);
+  } >DTCM_RAM
+
+  /* The program code and other data goes into MAIN */
+  .text :
+  {
+    . = ALIGN(4);
+    *(.text)           /* .text sections (code) */
+    *(.text*)          /* .text* sections (code) */
+    *(.rodata)         /* .rodata sections (constants, strings, etc.) */
+    *(.rodata*)        /* .rodata* sections (constants, strings, etc.) */
+    *(.glue_7)         /* glue arm to thumb code */
+    *(.glue_7t)        /* glue thumb to arm code */
+    *(.eh_frame)
+
+    KEEP (*(.init))
+    KEEP (*(.fini))
+
+    . = ALIGN(4);
+    _etext = .;        /* define a global symbols at end of code */
+  } >MAIN
+
+  /* Critical program code goes into ITCM RAM */
+  /* Copy specific fast-executing code to ITCM RAM */ 
+  tcm_code = LOADADDR(.tcm_code); 
+  .tcm_code :
+  {
+    . = ALIGN(4);
+    tcm_code_start = .; 
+    *(.tcm_code)
+    *(.tcm_code*)
+    . = ALIGN(4);
+    tcm_code_end = .; 
+  } >ITCM_RAM AT >MAIN
+
+  .ARM.extab   : 
+  { 
+    *(.ARM.extab* .gnu.linkonce.armextab.*) 
+  } >MAIN
+  
+  .ARM : 
+  {
+    __exidx_start = .;
+    *(.ARM.exidx*) __exidx_end = .;
+  } >MAIN
+
+  .pg_registry :
+  {
+    PROVIDE_HIDDEN (__pg_registry_start = .);
+    KEEP (*(.pg_registry))
+    KEEP (*(SORT(.pg_registry.*)))
+    PROVIDE_HIDDEN (__pg_registry_end = .);
+  } >MAIN
+  
+  .pg_resetdata :
+  {
+    PROVIDE_HIDDEN (__pg_resetdata_start = .);
+    KEEP (*(.pg_resetdata))
+    PROVIDE_HIDDEN (__pg_resetdata_end = .);
+  } >MAIN
+
+  /* used by the startup to initialize data */
+  _sidata = LOADADDR(.data);
+
+  /* Initialized data sections goes into RAM, load LMA copy after code */
+  .data :
+  {
+    . = ALIGN(4);
+    _sdata = .;        /* create a global symbol at data start */
+    *(.data)           /* .data sections */
+    *(.data*)          /* .data* sections */
+
+    . = ALIGN(4);
+    _edata = .;        /* define a global symbol at data end */
+  } >DTCM_RAM AT >MAIN
+
+  /* Non-critical program code goes into RAM */
+  /* Copy specific slow-executing code to RAM */ 
+  ram_code = LOADADDR(.ram_code); 
+  .ram_code :
+  {
+    . = ALIGN(4);
+    ram_code_start = .; 
+    *(.ram_code)
+    *(.ram_code*)
+    . = ALIGN(4);
+    ram_code_end = .; 
+  } >RAM AT >MAIN
+
+  /* Uninitialized data section */
+  . = ALIGN(4);
+  .bss (NOLOAD) :
+  {
+    /* This is used by the startup in order to initialize the .bss secion */
+    _sbss = .;         /* define a global symbol at bss start */
+    __bss_start__ = _sbss;
+    *(.bss)
+    *(SORT_BY_ALIGNMENT(.bss*))
+    *(COMMON)
+
+    . = ALIGN(4);
+    _ebss = .;         /* define a global symbol at bss end */
+    __bss_end__ = _ebss;
+  } >RAM
+
+  /* Uninitialized data section */
+  . = ALIGN(4);
+  .sram2 (NOLOAD) :
+  {
+    /* This is used by the startup in order to initialize the .sram2 secion */
+    _ssram2 = .;         /* define a global symbol at sram2 start */
+    __sram2_start__ = _ssram2;
+    *(.sram2)
+    *(SORT_BY_ALIGNMENT(.sram2*))
+
+    . = ALIGN(4);
+    _esram2 = .;         /* define a global symbol at sram2 end */
+    __sram2_end__ = _esram2;
+  } >RAM
+
+  /* used during startup to initialized fastram_data */
+  _sfastram_idata = LOADADDR(.fastram_data);
+
+  /* Initialized FAST_DATA section for unsuspecting developers */
+  .fastram_data :
+  {
+    . = ALIGN(4);
+    _sfastram_data = .;        /* create a global symbol at data start */
+    *(.fastram_data)           /* .data sections */
+    *(.fastram_data*)          /* .data* sections */
+
+    . = ALIGN(4);
+    _efastram_data = .;        /* define a global symbol at data end */
+  } >FASTRAM AT >MAIN
+
+  . = ALIGN(4);
+  .fastram_bss (NOLOAD) :
+  {
+    _sfastram_bss = .;
+    __fastram_bss_start__ = _sfastram_bss;
+    *(.fastram_bss)
+    *(SORT_BY_ALIGNMENT(.fastram_bss*))
+
+    . = ALIGN(4);
+    _efastram_bss = .;
+    __fastram_bss_end__ = _efastram_bss;
+  } >FASTRAM
+}
diff --git a/src/link/stm32_h723_common_post.ld b/src/link/stm32_h723_common_post.ld
new file mode 100644
index 00000000000..89b4224806f
--- /dev/null
+++ b/src/link/stm32_h723_common_post.ld
@@ -0,0 +1,44 @@
+SECTIONS
+{
+  .persistent_data (NOLOAD) :
+  {
+    __persistent_data_start__ = .;
+    *(.persistent_data)
+    . = ALIGN(4);
+    __persistent_data_end__ = .;
+  } >RAM
+
+  /* User_heap_stack section, used to check that there is enough RAM left */
+  _heap_stack_end = ORIGIN(STACKRAM)+LENGTH(STACKRAM) - 8; /* 8 bytes to allow for alignment */
+  _heap_stack_begin = _heap_stack_end - _Min_Stack_Size  - _Min_Heap_Size;
+  . = _heap_stack_begin;
+  ._user_heap_stack :
+  {
+    . = ALIGN(4);
+    PROVIDE ( end = . );
+    PROVIDE ( _end = . );
+    . = . + _Min_Heap_Size;
+    . = . + _Min_Stack_Size;
+    . = ALIGN(4);
+  } >STACKRAM = 0xa5
+
+  /* MEMORY_bank1 section, code must be located here explicitly            */
+  /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */
+  .memory_b1_text :
+  {
+    *(.mb1text)        /* .mb1text sections (code) */
+    *(.mb1text*)       /* .mb1text* sections (code)  */
+    *(.mb1rodata)      /* read-only data (constants) */
+    *(.mb1rodata*)
+  } >MEMORY_B1
+  
+    /* Remove information from the standard libraries */
+  /DISCARD/ :
+  {
+    libc.a ( * )
+    libm.a ( * )
+    libgcc.a ( * )
+  }
+
+  .ARM.attributes 0 : { *(.ARM.attributes) }
+}
diff --git a/src/link/stm32_h730_common.ld b/src/link/stm32_h730_common.ld
index a485128aa8b..d10f3d957fa 100644
--- a/src/link/stm32_h730_common.ld
+++ b/src/link/stm32_h730_common.ld
@@ -113,6 +113,19 @@ SECTIONS
     _edata = .;        /* define a global symbol at data end */
   } >DTCM_RAM AT >MAIN
 
+  /* Non-critical program code goes into RAM */
+  /* Copy specific slow-executing code to RAM */ 
+  ram_code = LOADADDR(.ram_code); 
+  .ram_code :
+  {
+    . = ALIGN(4);
+    ram_code_start = .; 
+    *(.ram_code)
+    *(.ram_code*)
+    . = ALIGN(4);
+    ram_code_end = .; 
+  } >RAM AT >MAIN
+
   /* Uninitialized data section */
   . = ALIGN(4);
   .bss (NOLOAD) :
diff --git a/src/link/stm32_ram_h723_exst.ld b/src/link/stm32_ram_h723_exst.ld
new file mode 100644
index 00000000000..c9f0bcd8741
--- /dev/null
+++ b/src/link/stm32_ram_h723_exst.ld
@@ -0,0 +1,143 @@
+/*
+*****************************************************************************
+**
+**  File        : stm32_ram_h723_exst.ld
+**
+**  Abstract    : Linker script for STM32H723xG Device with
+**                320K AXI-SRAM mapped onto AXI bus on D1 domain
+                       (Shared AXI/I-TCM 192KB is all configured as AXI-SRAM)
+**                 16K SRAM1 mapped on D2 domain
+**                 16K SRAM2 mapped on D2 domain
+**                 16K SRAM  mapped on D3 domain
+**                 64K ITCM
+**                128K DTCM
+**
+*****************************************************************************
+*/
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/*
+0x00000000 to 0x0000FFFF   64K ITCM
+0x20000000 to 0x2001FFFF  128K DTCM, main RAM
+0x24000000 to 0x2404FFFF  320K AXI SRAM, D1 domain
+0x30000000 to 0x30003FFF   16K SRAM1, D2 domain
+0x30004000 to 0x30007FFF   16K SRAM2, D2 domain
+0x38000000 to 0x38003FFF   16K SRAM4, D3 domain, unused
+0x38800000 to 0x38800FFF    4K BACKUP SRAM, Backup domain, unused
+
+0x08000000 to 0x0801FFFF  128K isr vector, startup code, firmware, no config! // FLASH_Sector_0
+*/
+
+/* 
+
+For H7 EXFL (External Flash) targets a binary is built that is placed on an external device.
+The bootloader will enable the memory mapped mode on the CPU which allows code to run directly from
+the external flash device.
+
+The bootloader then executes code at the CODE_RAM address. The address of CODE_RAM is fixed to 0x90010000
+and must not be changed.
+
+The initial CODE_RAM is sized at 1MB.
+
+*/
+
+/* see .exst section below */
+_exst_hash_size = 64;
+
+/* Specify the memory areas */
+MEMORY
+{
+    ITCM_RAM (rwx)    : ORIGIN = 0x00000000, LENGTH = 64K
+    DTCM_RAM (rwx)    : ORIGIN = 0x20000000, LENGTH = 128K
+    RAM (rwx)         : ORIGIN = 0x24000000, LENGTH = 128K + 192K /* 128K AXI SRAM + 192K ITCM & AXI = 320K */
+
+    D2_RAM (rwx)      : ORIGIN = 0x30000000, LENGTH = 32K /* SRAM1 16K + SRAM2 16K */
+    D3_RAM (rwx)      : ORIGIN = 0x38000000, LENGTH = 16K /* SRAM4 16K */
+
+    BACKUP_SRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K
+
+    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
+
+    OCTOSPI2 (rx)     : ORIGIN = 0x70000000, LENGTH = 256M
+    OCTOSPI1 (rx)     : ORIGIN = 0x90000000, LENGTH = 256M 
+    OCTOSPI1_CODE (rx): ORIGIN = ORIGIN(OCTOSPI1) + 1M, LENGTH = 1M - _exst_hash_size /* hard coded start address, as required by SPRACINGH7 boot loader, don't change! */
+    EXST_HASH (rx)    : ORIGIN = ORIGIN(OCTOSPI1_CODE) + LENGTH(OCTOSPI1_CODE), LENGTH = _exst_hash_size
+}
+
+REGION_ALIAS("STACKRAM", DTCM_RAM)
+REGION_ALIAS("FASTRAM", DTCM_RAM)
+REGION_ALIAS("MAIN", OCTOSPI1_CODE)
+
+REGION_ALIAS("VECTAB", MAIN)
+
+INCLUDE "stm32_h723_common.ld"
+
+SECTIONS
+{
+  /* used during startup to initialized dmaram_data */
+  _sdmaram_idata = LOADADDR(.dmaram_data);
+
+  . = ALIGN(32);
+  .dmaram_data :
+  {
+    PROVIDE(dmaram_start = .);
+    _sdmaram = .;
+    _dmaram_start__ = _sdmaram;
+    _sdmaram_data = .;        /* create a global symbol at data start */
+    *(.dmaram_data)           /* .data sections */
+    *(.dmaram_data*)          /* .data* sections */
+    . = ALIGN(32);
+    _edmaram_data = .;        /* define a global symbol at data end */
+  } >RAM AT >MAIN
+
+  . = ALIGN(32);
+  .dmaram_bss (NOLOAD) :
+  {
+    _sdmaram_bss = .;
+    __dmaram_bss_start__ = _sdmaram_bss;
+    *(.dmaram_bss)
+    *(SORT_BY_ALIGNMENT(.dmaram_bss*))
+    . = ALIGN(32);
+    _edmaram_bss = .;
+    __dmaram_bss_end__ = _edmaram_bss;
+  } >RAM
+
+  . = ALIGN(32);
+  .DMA_RAM (NOLOAD) :
+  {
+    KEEP(*(.DMA_RAM))
+    PROVIDE(dmaram_end = .);
+    _edmaram = .;
+    _dmaram_end__ = _edmaram;
+  } >RAM
+  
+  .DMA_RW_D2 (NOLOAD) :
+  {
+    . = ALIGN(32);
+    PROVIDE(dmarw_start = .);
+    _sdmarw = .;
+    _dmarw_start__ = _sdmarw;
+    KEEP(*(.DMA_RW))
+    PROVIDE(dmarw_end = .);
+    _edmarw = .;
+    _dmarw_end__ = _edmarw;
+  } >D2_RAM
+
+  .DMA_RW_AXI (NOLOAD) :
+  {
+    . = ALIGN(32);
+    PROVIDE(dmarwaxi_start = .);
+    _sdmarwaxi = .;
+    _dmarwaxi_start__ = _sdmarwaxi;
+    KEEP(*(.DMA_RW_AXI))
+    PROVIDE(dmarwaxi_end = .);
+    _edmarwaxi = .;
+    _dmarwaxi_end__ = _edmarwaxi;
+  } >RAM
+}
+
+INCLUDE "stm32_h723_common_post.ld"
+INCLUDE "stm32_ram_h723_exst_post.ld"
+
diff --git a/src/link/stm32_ram_h723_exst_post.ld b/src/link/stm32_ram_h723_exst_post.ld
new file mode 100644
index 00000000000..97ed6c28f3a
--- /dev/null
+++ b/src/link/stm32_ram_h723_exst_post.ld
@@ -0,0 +1,29 @@
+SECTIONS
+{
+  /* Create space for a hash.  Currently an MD5 has is used, which is 16   */
+  /* bytes long. however the last 64 bytes are RESERVED for hash related   */
+  .exst_hash :
+  {
+    /* 64 bytes is the size of an MD5 hashing block size.                  */
+    . = ORIGIN(EXST_HASH);
+
+    BYTE(0x00); /* block format */
+    BYTE(0x00); /* Checksum method, 0x00 = MD5 hash */
+    BYTE(0x00); /* Reserved */
+    BYTE(0x00); /* Reserved */
+    
+    /* Fill the last 60 bytes with data, including an empty hash aligned   */
+    
+    /* to the last 16 bytes.                                               */
+    FILL(0x00000000); /* Reserved */
+    
+    . = ORIGIN(EXST_HASH) + LENGTH(EXST_HASH) - 16;
+    __md5_hash_address__ = .;
+    LONG(0x00000000);
+    LONG(0x00000000);
+    LONG(0x00000000);
+    LONG(0x00000000);
+    . = ORIGIN(EXST_HASH) + LENGTH(EXST_HASH);
+    __firmware_end__ = .;
+  } >EXST_HASH
+}
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 514ad495845..294dae98931 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1535,12 +1535,14 @@ static bool blackboxWriteSysinfo(void)
 
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d",      gpsRescueConfig()->initialAltitudeM)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_SPEED, "%d",    gpsRescueConfig()->rescueGroundspeed)
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX, "%d", gpsRescueConfig()->angle)
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d",        gpsRescueConfig()->rollMix)
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d",    gpsRescueConfig()->pitchCutoffHz)
 
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d",    gpsRescueConfig()->descentDistanceM)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d",    gpsRescueConfig()->descendRate)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_ALT, "%d",     gpsRescueConfig()->targetLandingAltitudeM)
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold)
 
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, "%d",    gpsRescueConfig()->throttleMin)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, "%d",    gpsRescueConfig()->throttleMax)
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 6bfe05a0992..deb5a9dbb27 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -108,4 +108,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "VTX_MSP",
     "GPS_DOP",
     "FAILSAFE",
+    "DSHOT_TELEMETRY_COUNTS"
 };
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index f43cebd1175..1413badcb41 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -106,6 +106,7 @@ typedef enum {
     DEBUG_VTX_MSP,
     DEBUG_GPS_DOP,
     DEBUG_FAILSAFE,
+    DEBUG_DSHOT_TELEMETRY_COUNTS,
     DEBUG_COUNT
 } debugType_e;
 
diff --git a/src/main/build/version.c b/src/main/build/version.c
index 99d5c45d8fc..c86fb5fc873 100644
--- a/src/main/build/version.c
+++ b/src/main/build/version.c
@@ -26,3 +26,15 @@ const char * const targetName = __TARGET__;
 const char * const shortGitRevision = __REVISION__;
 const char * const buildDate = __DATE__;
 const char * const buildTime = __TIME__;
+
+#ifdef BUILD_KEY
+    const char * const buildKey = STR(BUILD_KEY);
+#else
+    const char * const buildKey = NULL;
+#endif
+
+#if defined(BUILD_KEY) && defined(RELEASE_NAME)
+    const char * const releaseName = STR(RELEASE_NAME);
+#else
+    const char * const releaseName = NULL;
+#endif
diff --git a/src/main/build/version.h b/src/main/build/version.h
index 65f93eae1ab..53239cdf8e7 100644
--- a/src/main/build/version.h
+++ b/src/main/build/version.h
@@ -26,7 +26,7 @@
 #define FC_FIRMWARE_IDENTIFIER      "BTFL"
 #define FC_VERSION_MAJOR            4  // increment when a major release is made (big new feature, etc)
 #define FC_VERSION_MINOR            4  // increment when a minor release is made (small new feature, change etc)
-#define FC_VERSION_PATCH_LEVEL      0  // increment when a bug is fixed
+#define FC_VERSION_PATCH_LEVEL      4  // increment when a bug is fixed
 
 #define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL)
 
@@ -42,3 +42,6 @@ extern const char* const buildDate;  // "MMM DD YYYY" MMM = Jan/Feb/...
 extern const char* const buildTime;  // "HH:MM:SS"
 
 #define MSP_API_VERSION_STRING STR(API_VERSION_MAJOR) "." STR(API_VERSION_MINOR)
+
+extern const char* const buildKey;
+extern const char* const releaseName;
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 95208d7c59d..9c3c292152e 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -2582,8 +2582,8 @@ static void cliFlashRead(const char *cmdName, char *cmdline)
         cliPrintLinefeed();
     }
 }
-#endif
-#endif
+#endif // USE_FLASH_TOOLS
+#endif // USE_FLASHFS
 
 #ifdef USE_VTX_CONTROL
 static void printVtx(dumpFlags_t dumpMask, const vtxConfig_t *vtxConfig, const vtxConfig_t *vtxConfigDefault, const char *headingStr)
@@ -4813,13 +4813,13 @@ static void cliStatus(const char *cmdName, char *cmdline)
     cliPrintLinef("OSD: %s (%u x %u)", lookupTableOsdDisplayPortDevice[displayPortDeviceType], osdDisplayPort->cols, osdDisplayPort->rows);
 #endif
 
-#ifdef BUILD_KEY
-    cliPrintf("BUILD KEY: %s", STR(BUILD_KEY));
-#ifdef RELEASE_NAME
-    cliPrintf(" (%s)", STR(RELEASE_NAME));
-#endif
+if (buildKey) {
+    cliPrintf("BUILD KEY: %s", buildKey);
+    if (releaseName) {
+        cliPrintf(" (%s)", releaseName);
+    }
     cliPrintLinefeed();
-#endif
+}
 
     // Uptime and wall clock
 
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index f406524b72a..1aaba47b692 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -1010,19 +1010,21 @@ const clivalue_t valueTable[] = {
 
 #ifdef USE_GPS_RESCUE
     // PG_GPS_RESCUE
-    { PARAM_NAME_GPS_RESCUE_MIN_START_DIST,  VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) },
+    { PARAM_NAME_GPS_RESCUE_MIN_START_DIST,  VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 30 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) },
     { PARAM_NAME_GPS_RESCUE_ALT_MODE,        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_ALT_MODE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, altitudeMode) },
     { PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB,   VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueAltitudeBufferM) },
     { PARAM_NAME_GPS_RESCUE_ASCEND_RATE,     VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 2500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, ascendRate) },
 
     { PARAM_NAME_GPS_RESCUE_RETURN_ALT,      VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 2, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) },
     { PARAM_NAME_GPS_RESCUE_RETURN_SPEED,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) },
-    { PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX, VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) },
+    { PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 80 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, maxRescueAngle) },
     { PARAM_NAME_GPS_RESCUE_ROLL_MIX,        VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rollMix) },
+    { PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF,    VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, pitchCutoffHz) },
 
-    { PARAM_NAME_GPS_RESCUE_DESCENT_DIST,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) },
+    { PARAM_NAME_GPS_RESCUE_DESCENT_DIST,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) },
     { PARAM_NAME_GPS_RESCUE_DESCEND_RATE,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) },
     { PARAM_NAME_GPS_RESCUE_LANDING_ALT,     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) },
+    { PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, disarmThreshold) },
 
     { PARAM_NAME_GPS_RESCUE_THROTTLE_MIN,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) },
     { PARAM_NAME_GPS_RESCUE_THROTTLE_MAX,    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },
@@ -1180,7 +1182,7 @@ const clivalue_t valueTable[] = {
 #ifdef USE_FEEDFORWARD
     { PARAM_NAME_FEEDFORWARD_TRANSITION,     VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_transition) },
     { PARAM_NAME_FEEDFORWARD_AVERAGING,      VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FEEDFORWARD_AVERAGING }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_averaging) },
-    { PARAM_NAME_FEEDFORWARD_SMOOTH_FACTOR,  VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) },
+    { PARAM_NAME_FEEDFORWARD_SMOOTH_FACTOR,  VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 95}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) },
     { PARAM_NAME_FEEDFORWARD_JITTER_FACTOR,  VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_jitter_factor) },
     { PARAM_NAME_FEEDFORWARD_BOOST,          VAR_UINT8 | PROFILE_VALUE,  .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_boost) },
     { PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) },
@@ -1592,7 +1594,7 @@ const clivalue_t valueTable[] = {
     { "usb_msc_pin_pullup", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_USB_CONFIG, offsetof(usbDev_t, mscButtonUsePullup) },
 #endif
 // PG_FLASH_CONFIG
-#ifdef USE_FLASH_CHIP
+#ifdef USE_FLASH_SPI
     { "flash_spi_bus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_FLASH_CONFIG, offsetof(flashConfig_t, spiDevice) },
 #endif
 // RCDEVICE
diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 35179122244..c4578ffd303 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -161,9 +161,10 @@ bool cmsDisplayPortSelect(displayPort_t *instance)
 //   13 cols x 9 rows, top row printed as a Bold Heading
 //   Needs the "smallScreen" adaptions
 
-#define CMS_MAX_ROWS 16
+#define CMS_MAX_ROWS 31
 
 #define NORMAL_SCREEN_MIN_COLS 18      // Less is a small screen
+#define NORMAL_SCREEN_MAX_COLS 30      // More is a large screen
 static bool    smallScreen;
 static uint8_t leftMenuColumn;
 static uint8_t rightMenuColumn;
@@ -389,7 +390,7 @@ static int cmsDrawMenuItemValue(displayPort_t *pDisplay, char *buff, uint8_t row
 #else
     colpos = smallScreen ? rightMenuColumn - maxSize : rightMenuColumn;
 #endif
-    cnt = cmsDisplayWrite(pDisplay, colpos, row, DISPLAYPORT_ATTR_NORMAL, buff);
+    cnt = cmsDisplayWrite(pDisplay, colpos, row, DISPLAYPORT_SEVERITY_NORMAL, buff);
     return cnt;
 }
 
@@ -590,7 +591,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t
     case OME_Label:
         if (IS_PRINTVALUE(*flags) && p->data) {
             // A label with optional string, immediately following text
-            cnt = cmsDisplayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, DISPLAYPORT_ATTR_NORMAL, p->data);
+            cnt = cmsDisplayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, DISPLAYPORT_SEVERITY_NORMAL, p->data);
             CLR_PRINTVALUE(*flags);
         }
         break;
@@ -606,9 +607,9 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t
 #ifdef CMS_MENU_DEBUG
         // Shouldn't happen. Notify creator of this menu content
 #ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
-        cnt = cmsDisplayWrite(pDisplay, rightMenuColumn - 6, row, DISPLAYPORT_ATTR_NORMAL, "BADENT");
+        cnt = cmsDisplayWrite(pDisplay, rightMenuColumn - 6, row, DISPLAYPORT_SEVERITY_NORMAL, "BADENT");
 #else
-        cnt = cmsDisplayWrite(pDisplay, rightMenuColumn, row, DISPLAYPORT_ATTR_NORMAL, "BADENT");
+        cnt = cmsDisplayWrite(pDisplay, rightMenuColumn, row, DISPLAYPORT_SEVERITY_NORMAL, "BADENT");
 #endif
 #endif
         break;
@@ -747,7 +748,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
 #endif
 
     if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) {
-        room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + pDisplay->cursorRow * linesPerMenuItem, DISPLAYPORT_ATTR_NORMAL, " ");
+        room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + pDisplay->cursorRow * linesPerMenuItem, DISPLAYPORT_SEVERITY_NORMAL, " ");
     }
 
     if (room < 30) {
@@ -755,7 +756,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
     }
 
     if (pDisplay->cursorRow != currentCtx.cursorRow) {
-        room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + currentCtx.cursorRow * linesPerMenuItem, DISPLAYPORT_ATTR_NORMAL, ">");
+        room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + currentCtx.cursorRow * linesPerMenuItem, DISPLAYPORT_SEVERITY_NORMAL, ">");
         pDisplay->cursorRow = currentCtx.cursorRow;
     }
 
@@ -777,7 +778,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
         if (IS_PRINTLABEL(runtimeEntryFlags[i])) {
             uint8_t coloff = leftMenuColumn;
             coloff += ((p->flags & OSD_MENU_ELEMENT_MASK) == OME_Label) ? 0 : 1;
-            room -= cmsDisplayWrite(pDisplay, coloff, top + i * linesPerMenuItem, DISPLAYPORT_ATTR_NORMAL, p->text);
+            room -= cmsDisplayWrite(pDisplay, coloff, top + i * linesPerMenuItem, DISPLAYPORT_SEVERITY_NORMAL, p->text);
             CLR_PRINTLABEL(runtimeEntryFlags[i]);
             if (room < 30) {
                 return;
@@ -787,7 +788,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
 
         // Highlight values overridden by sliders
         if (rowSliderOverride(p->flags)) {
-            displayWriteChar(pDisplay, leftMenuColumn - 1, top + i * linesPerMenuItem, DISPLAYPORT_ATTR_NORMAL, 'S');
+            displayWriteChar(pDisplay, leftMenuColumn - 1, top + i * linesPerMenuItem, DISPLAYPORT_SEVERITY_NORMAL, 'S');
         }
 
     // Print values
@@ -810,12 +811,12 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
     // simple text device and use the '^' (carat) and 'V' for arrow approximations.
     if (displayWasCleared && leftMenuColumn > 0) {      // make sure there's room to draw the symbol
         if (currentCtx.page > 0) {
-            const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_NORTH : '^';
-            displayWriteChar(pDisplay, leftMenuColumn - 1, top, DISPLAYPORT_ATTR_NORMAL, symbol);
+            const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_SMALL_UP : '^';
+            displayWriteChar(pDisplay, leftMenuColumn - 1, top, DISPLAYPORT_SEVERITY_NORMAL, symbol);
         }
          if (currentCtx.page < pageCount - 1) {
-            const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_SOUTH : 'V';
-            displayWriteChar(pDisplay, leftMenuColumn - 1, top + pageMaxRow, DISPLAYPORT_ATTR_NORMAL, symbol);
+            const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_SMALL_DOWN : 'v';
+            displayWriteChar(pDisplay, leftMenuColumn - 1, top + pageMaxRow, DISPLAYPORT_SEVERITY_NORMAL, symbol);
         }
     }
 
@@ -936,12 +937,21 @@ void cmsMenuOpen(void)
     } else {
       smallScreen       = false;
       linesPerMenuItem  = 1;
-      leftMenuColumn    = (pCurrentDisplay->cols / 2) - 13;
+      if (pCurrentDisplay->cols <= NORMAL_SCREEN_MAX_COLS) {
+          leftMenuColumn    = 2;
 #ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
-      rightMenuColumn   = (pCurrentDisplay->cols / 2) + 13;
+          rightMenuColumn   = pCurrentDisplay->cols - 2;
 #else
-      rightMenuColumn   = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN;
+          rightMenuColumn   = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN;
 #endif
+      } else {
+          leftMenuColumn    = (pCurrentDisplay->cols / 2) - 13;
+#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
+          rightMenuColumn   = (pCurrentDisplay->cols / 2) + 13;
+#else
+          rightMenuColumn   = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN;
+#endif
+      }
       maxMenuItems      = pCurrentDisplay->rows - 2;
     }
 
@@ -1004,7 +1014,7 @@ const void *cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
 
     if ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT) || (exitType == CMS_POPUP_EXITREBOOT)) {
         displayClearScreen(pDisplay, DISPLAY_CLEAR_WAIT);
-        cmsDisplayWrite(pDisplay, 5, 3, DISPLAYPORT_ATTR_NORMAL, "REBOOTING...");
+        cmsDisplayWrite(pDisplay, 5, 3, DISPLAYPORT_SEVERITY_NORMAL, "REBOOTING...");
 
         // Flush display
         displayRedraw(pDisplay);
diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c
index 6548eca9601..7f7e3846796 100644
--- a/src/main/cms/cms_menu_blackbox.c
+++ b/src/main/cms/cms_menu_blackbox.c
@@ -170,7 +170,7 @@ static const void *cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
     }
 
     displayClearScreen(pDisplay, DISPLAY_CLEAR_WAIT);
-    displayWrite(pDisplay, 5, 3, DISPLAYPORT_ATTR_INFO, "ERASING FLASH...");
+    displayWrite(pDisplay, 5, 3, DISPLAYPORT_SEVERITY_INFO, "ERASING FLASH...");
     displayRedraw(pDisplay);
 
     flashfsEraseCompletely();
diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c
index a68ac9f5649..d3350e1b9f9 100644
--- a/src/main/cms/cms_menu_gps_rescue.c
+++ b/src/main/cms/cms_menu_gps_rescue.c
@@ -63,6 +63,7 @@ static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueCo
 static uint8_t gpsRescueConfig_yawP;
 static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
 
+static uint8_t gpsRescueConfig_pitchCutoffHz;
 
 static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
 {
@@ -78,6 +79,7 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
     gpsRescueConfig_velI = gpsRescueConfig()->velI;
     gpsRescueConfig_velD = gpsRescueConfig()->velD;
 
+    gpsRescueConfig_pitchCutoffHz = gpsRescueConfig()->pitchCutoffHz;
     return NULL;
 }
 
@@ -96,6 +98,8 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En
     gpsRescueConfigMutable()->velI = gpsRescueConfig_velI;
     gpsRescueConfigMutable()->velD = gpsRescueConfig_velD;
 
+    gpsRescueConfigMutable()->pitchCutoffHz = gpsRescueConfig_pitchCutoffHz;
+
     return NULL;
 }
 
@@ -113,6 +117,8 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] =
     { "VELOCITY I",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velI, 0, 255, 1 } },
     { "VELOCITY D",        OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velD, 0, 255, 1 } },
 
+    { "SMOOTHING",         OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_pitchCutoffHz, 10, 255, 1 } },
+
     {"BACK", OME_Back, NULL, NULL},
     {NULL, OME_END, NULL, NULL}
 };
@@ -139,7 +145,7 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
 
     gpsRescueConfig_initialAltitudeM = gpsRescueConfig()->initialAltitudeM;
     gpsRescueConfig_rescueGroundspeed = gpsRescueConfig()->rescueGroundspeed;
-    gpsRescueConfig_angle = gpsRescueConfig()->angle;
+    gpsRescueConfig_angle = gpsRescueConfig()->maxRescueAngle;
 
     gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
     gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
@@ -167,7 +173,7 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
 
     gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM;
     gpsRescueConfigMutable()->rescueGroundspeed = gpsRescueConfig_rescueGroundspeed;
-    gpsRescueConfigMutable()->angle = gpsRescueConfig_angle;
+    gpsRescueConfigMutable()->maxRescueAngle = gpsRescueConfig_angle;
 
     gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
     gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 9bd7c6cfff2..c42675aec40 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -661,7 +661,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
 #ifdef USE_FEEDFORWARD
     { "FF TRANSITION", OME_FLOAT,  NULL, &(OSD_FLOAT_t)  { &cmsx_feedforward_transition,        0,    100,   1, 10 } },
     { "FF AVERAGING",  OME_TAB,    NULL, &(OSD_TAB_t)    { &cmsx_feedforward_averaging,         4, lookupTableFeedforwardAveraging} },
-    { "FF SMOOTHNESS", OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_smooth_factor,     0,     75,   1  }    },
+    { "FF SMOOTHNESS", OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_smooth_factor,     0,     95,   1  }    },
     { "FF JITTER",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_jitter_factor,     0,     20,   1  }    },
     { "FF BOOST",      OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_feedforward_boost,             0,     50,   1  }    },
 #endif
diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c
index 67c39f52ff8..c10bbc341f9 100644
--- a/src/main/common/sdft.c
+++ b/src/main/common/sdft.c
@@ -55,7 +55,7 @@ void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numB
     sdft->endBin = constrain(endBin, sdft->startBin, SDFT_BIN_COUNT - 1);
 
     sdft->numBatches = MAX(numBatches, 1);
-    sdft->batchSize = (sdft->endBin - sdft->startBin) / sdft->numBatches + 1;  // batchSize = ceil(numBins / numBatches)
+    sdft->batchSize = (sdft->endBin - sdft->startBin + 1) / sdft->numBatches;
 
     for (int i = 0; i < SDFT_SAMPLE_SIZE; i++) {
         sdft->samples[i] = 0.0f;
@@ -183,7 +183,7 @@ static FAST_CODE void applySqrt(const sdft_t *sdft, float *data)
 }
 
 
-// Needed for proper windowing at the edges of startBin and endBin
+// Needed for proper windowing at the edges of active range
 static FAST_CODE void updateEdges(sdft_t *sdft, const float value, const int batchIdx)
 {
     // First bin outside of lower range
diff --git a/src/main/config/config.c b/src/main/config/config.c
index eb5931eb712..1aa4e9220e3 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -630,8 +630,8 @@ void validateAndFixGyroConfig(void)
         // check for looptime restrictions based on motor protocol. Motor times have safety margin
         float motorUpdateRestriction;
 
-#if defined(STM32F411xE)
-        /* If bidirectional DSHOT is being used on an F411 then force DSHOT300. The motor update restrictions then applied
+#if defined(STM32F4)|| defined(STM32G4)
+        /* If bidirectional DSHOT is being used on an F4 or G4 then force DSHOT300. The motor update restrictions then applied
          * will automatically consider the loop time and adjust pid_process_denom appropriately
          */
         if (motorConfig()->dev.useDshotTelemetry && (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_DSHOT600)) {
diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index d9adb982ea8..1e166cab7b7 100644
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -83,8 +83,8 @@ typedef struct {
     uint32_t word;
 } PG_PACKED packingTest_t;
 
-#if defined(CONFIG_IN_EXTERNAL_FLASH)
-bool loadEEPROMFromExternalFlash(void)
+#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
+MMFLASH_CODE bool loadEEPROMFromExternalFlash(void)
 {
     const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG);
     const flashGeometry_t *flashGeometry = flashGetGeometry();
@@ -95,7 +95,9 @@ bool loadEEPROMFromExternalFlash(void)
     int bytesRead = 0;
 
     bool success = false;
-
+#ifdef CONFIG_IN_MEMORY_MAPPED_FLASH
+    flashMemoryMappedModeDisable();
+#endif
     do {
         bytesRead = flashReadBytes(flashStartAddress + totalBytesRead, &eepromData[totalBytesRead], EEPROM_SIZE - totalBytesRead);
         if (bytesRead > 0) {
@@ -103,9 +105,55 @@ bool loadEEPROMFromExternalFlash(void)
             success = (totalBytesRead == EEPROM_SIZE);
         }
     } while (!success && bytesRead > 0);
+#ifdef CONFIG_IN_MEMORY_MAPPED_FLASH
+    flashMemoryMappedModeEnable();
+#endif
 
     return success;
 }
+
+#ifdef CONFIG_IN_MEMORY_MAPPED_FLASH
+MMFLASH_CODE_NOINLINE void saveEEPROMToMemoryMappedFlash(void)
+{
+    const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG);
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+
+    uint32_t flashSectorSize = flashGeometry->sectorSize;
+    uint32_t flashPageSize = flashGeometry->pageSize;
+
+    uint32_t flashStartAddress = flashPartition->startSector * flashGeometry->sectorSize;
+
+    uint32_t bytesRemaining = EEPROM_SIZE;
+    uint32_t offset = 0;
+
+    flashMemoryMappedModeDisable();
+
+    do {
+        uint32_t flashAddress = flashStartAddress + offset;
+
+        uint32_t bytesToWrite = bytesRemaining;
+        if (bytesToWrite > flashPageSize) {
+            bytesToWrite = flashPageSize;
+        }
+
+        bool onSectorBoundary = flashAddress % flashSectorSize == 0;
+        if (onSectorBoundary) {
+            flashEraseSector(flashAddress);
+        }
+
+        flashPageProgram(flashAddress, (uint8_t *)&eepromData[offset], bytesToWrite, NULL);
+
+        bytesRemaining -= bytesToWrite;
+        offset += bytesToWrite;
+    } while (bytesRemaining > 0);
+
+    flashWaitForReady();
+
+    flashMemoryMappedModeEnable();
+}
+#endif
+
+
 #elif defined(CONFIG_IN_SDCARD)
 
 enum {
@@ -255,7 +303,7 @@ void initEEPROM(void)
 
 #if defined(CONFIG_IN_FILE)
     loadEEPROMFromFile();
-#elif defined(CONFIG_IN_EXTERNAL_FLASH)
+#elif defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
     bool eepromLoaded = loadEEPROMFromExternalFlash();
     if (!eepromLoaded) {
         // Flash read failed - just die now
@@ -336,7 +384,7 @@ uint16_t getEEPROMConfigSize(void)
 
 size_t getEEPROMStorageSize(void)
 {
-#if defined(CONFIG_IN_EXTERNAL_FLASH)
+#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
 
     const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG);
     return FLASH_PARTITION_SECTOR_COUNT(flashPartition) * flashGetGeometry()->sectorSize;
@@ -464,7 +512,7 @@ void writeConfigToEEPROM(void)
         if (writeSettingsToEEPROM()) {
             success = true;
 
-#ifdef CONFIG_IN_EXTERNAL_FLASH
+#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
             // copy it back from flash to the in-memory buffer.
             success = loadEEPROMFromExternalFlash();
 #endif
diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c
index b66c1ba793a..0d4ef55faf8 100644
--- a/src/main/config/config_streamer.c
+++ b/src/main/config/config_streamer.c
@@ -36,7 +36,7 @@ uint8_t eepromData[EEPROM_SIZE];
 #endif
 
 
-#if (defined(STM32H750xx) || defined(STM32H730xx)) && !(defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_RAM) || defined(CONFIG_IN_SDCARD))
+#if (defined(STM32H750xx) || defined(STM32H730xx)) && !(defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH) || defined(CONFIG_IN_RAM) || defined(CONFIG_IN_SDCARD))
 #error "The configured MCU only has one flash page which contains the bootloader, no spare flash pages available, use external storage for persistent config or ram for target testing"
 #endif
 // @todo this is not strictly correct for F4/F7, where sector sizes are variable
@@ -400,7 +400,7 @@ static int write_word(config_streamer_t *c, config_streamer_buffer_align_type_t
 
     flashPageProgramContinue(buffers, bufferSizes, 1);
 
-#elif defined(CONFIG_IN_RAM) || defined(CONFIG_IN_SDCARD)
+#elif defined(CONFIG_IN_RAM) || defined(CONFIG_IN_SDCARD) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
     if (c->address == (uintptr_t)&eepromData[0]) {
         memset(eepromData, 0, sizeof(eepromData));
     }
@@ -541,11 +541,13 @@ int config_streamer_finish(config_streamer_t *c)
 {
     if (c->unlocked) {
 #if defined(CONFIG_IN_SDCARD)
-        bool saveEEPROMToSDCard(void); // XXX forward declaration to avoid circular dependency between config_streamer / config_eeprom
+        bool saveEEPROMToSDCard(void); // forward declaration to avoid circular dependency between config_streamer / config_eeprom
         saveEEPROMToSDCard();
-        // TODO overwrite the data in the file on the SD card.
 #elif defined(CONFIG_IN_EXTERNAL_FLASH)
         flashFlush();
+#elif defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
+        void saveEEPROMToMemoryMappedFlash(void); // forward declaration to avoid circular dependency between config_streamer / config_eeprom
+        saveEEPROMToMemoryMappedFlash();
 #elif defined(CONFIG_IN_RAM)
         // NOP
 #elif defined(CONFIG_IN_FILE)
diff --git a/src/main/config/config_streamer.h b/src/main/config/config_streamer.h
index 20b3861a2a9..98680089980 100644
--- a/src/main/config/config_streamer.h
+++ b/src/main/config/config_streamer.h
@@ -26,7 +26,7 @@
 // Streams data out to the EEPROM, padding to the write size as
 // needed, and updating the checksum as it goes.
 
-#ifdef CONFIG_IN_EXTERNAL_FLASH
+#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
 #define CONFIG_STREAMER_BUFFER_SIZE 8 // Must not be greater than the smallest flash page size of all compiled-in flash devices.
 typedef uint32_t config_streamer_buffer_align_type_t;
 #elif defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx)
diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
index 54d3f8e4786..33e889affc6 100644
--- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
+++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
@@ -48,9 +48,22 @@
 // 24 MHz max SPI frequency
 #define ICM426XX_MAX_SPI_CLK_HZ 24000000
 
-#define ICM426XX_RA_PWR_MGMT0                       0x4E
+#define ICM426XX_RA_REG_BANK_SEL                    0x76
+#define ICM426XX_BANK_SELECT0                       0x00
+#define ICM426XX_BANK_SELECT1                       0x01
+#define ICM426XX_BANK_SELECT2                       0x02
+#define ICM426XX_BANK_SELECT3                       0x03
+#define ICM426XX_BANK_SELECT4                       0x04
+
+// Fix for stalls in gyro output. See https://github.com/ArduPilot/ardupilot/pull/25332
+#define ICM426XX_INTF_CONFIG1                       0x4D
+#define ICM426XX_INTF_CONFIG1_AFSR_MASK             0xC0
+#define ICM426XX_INTF_CONFIG1_AFSR_DISABLE          0x40
+
+#define ICM426XX_RA_PWR_MGMT0                       0x4E  // User Bank 0
 #define ICM426XX_PWR_MGMT0_ACCEL_MODE_LN            (3 << 0)
 #define ICM426XX_PWR_MGMT0_GYRO_MODE_LN             (3 << 2)
+#define ICM426XX_PWR_MGMT0_GYRO_ACCEL_MODE_OFF      ((0 << 0) | (0 << 2))
 #define ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF         (0 << 5)
 #define ICM426XX_PWR_MGMT0_TEMP_DISABLE_ON          (1 << 5)
 
@@ -58,22 +71,22 @@
 #define ICM426XX_RA_ACCEL_CONFIG0                   0x50
 
 // --- Registers for gyro and acc Anti-Alias Filter ---------
-#define ICM426XX_RA_GYRO_CONFIG_STATIC3             0x0C
-#define ICM426XX_RA_GYRO_CONFIG_STATIC4             0x0D
-#define ICM426XX_RA_GYRO_CONFIG_STATIC5             0x0E
-#define ICM426XX_RA_ACCEL_CONFIG_STATIC2            0x03
-#define ICM426XX_RA_ACCEL_CONFIG_STATIC3            0x04
-#define ICM426XX_RA_ACCEL_CONFIG_STATIC4            0x05
+#define ICM426XX_RA_GYRO_CONFIG_STATIC3             0x0C  // User Bank 1
+#define ICM426XX_RA_GYRO_CONFIG_STATIC4             0x0D  // User Bank 1
+#define ICM426XX_RA_GYRO_CONFIG_STATIC5             0x0E  // User Bank 1
+#define ICM426XX_RA_ACCEL_CONFIG_STATIC2            0x03  // User Bank 2
+#define ICM426XX_RA_ACCEL_CONFIG_STATIC3            0x04  // User Bank 2
+#define ICM426XX_RA_ACCEL_CONFIG_STATIC4            0x05  // User Bank 2
 // --- Register & setting for gyro and acc UI Filter --------
-#define ICM426XX_RA_GYRO_ACCEL_CONFIG0              0x52
-#define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY       (14 << 4)
-#define ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY        (14 << 0)
+#define ICM426XX_RA_GYRO_ACCEL_CONFIG0              0x52  // User Bank 0
+#define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY       (15 << 4) 
+#define ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY        (15 << 0)
 // ----------------------------------------------------------
 
-#define ICM426XX_RA_GYRO_DATA_X1                    0x25
-#define ICM426XX_RA_ACCEL_DATA_X1                   0x1F
+#define ICM426XX_RA_GYRO_DATA_X1                    0x25  // User Bank 0
+#define ICM426XX_RA_ACCEL_DATA_X1                   0x1F  // User Bank 0
 
-#define ICM426XX_RA_INT_CONFIG                      0x14
+#define ICM426XX_RA_INT_CONFIG                      0x14  // User Bank 0
 #define ICM426XX_INT1_MODE_PULSED                   (0 << 2)
 #define ICM426XX_INT1_MODE_LATCHED                  (1 << 2)
 #define ICM426XX_INT1_DRIVE_CIRCUIT_OD              (0 << 1)
@@ -81,13 +94,13 @@
 #define ICM426XX_INT1_POLARITY_ACTIVE_LOW           (0 << 0)
 #define ICM426XX_INT1_POLARITY_ACTIVE_HIGH          (1 << 0)
 
-#define ICM426XX_RA_INT_CONFIG0                     0x63
+#define ICM426XX_RA_INT_CONFIG0                     0x63  // User Bank 0
 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR           ((0 << 5) || (0 << 4))
 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_F1BR          ((1 << 5) || (0 << 4))
 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR  ((1 << 5) || (1 << 4))
 
-#define ICM426XX_RA_INT_CONFIG1                     0x64
+#define ICM426XX_RA_INT_CONFIG1                     0x64   // User Bank 0
 #define ICM426XX_INT_ASYNC_RESET_BIT                4
 #define ICM426XX_INT_TDEASSERT_DISABLE_BIT          5
 #define ICM426XX_INT_TDEASSERT_ENABLED              (0 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
@@ -96,7 +109,7 @@
 #define ICM426XX_INT_TPULSE_DURATION_100            (0 << ICM426XX_INT_TPULSE_DURATION_BIT)
 #define ICM426XX_INT_TPULSE_DURATION_8              (1 << ICM426XX_INT_TPULSE_DURATION_BIT)
 
-#define ICM426XX_RA_INT_SOURCE0                     0x65
+#define ICM426XX_RA_INT_SOURCE0                     0x65  // User Bank 0
 #define ICM426XX_UI_DRDY_INT1_EN_DISABLED           (0 << 3)
 #define ICM426XX_UI_DRDY_INT1_EN_ENABLED            (1 << 3)
 
@@ -130,14 +143,23 @@ static uint8_t odrLUT[ODR_CONFIG_COUNT] = {  // see GYRO_ODR in section 5.6
     [ODR_CONFIG_1K] = 6,
 };
 
-// Possible gyro Anti-Alias Filter (AAF) cutoffs
-static aafConfig_t aafLUT[AAF_CONFIG_COUNT] = {  // see table in section 5.3
+// Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
+static aafConfig_t aafLUT42688[AAF_CONFIG_COUNT] = {  // see table in section 5.3
     [AAF_CONFIG_258HZ]  = {  6,   36, 10 },
     [AAF_CONFIG_536HZ]  = { 12,  144,  8 },
     [AAF_CONFIG_997HZ]  = { 21,  440,  6 },
     [AAF_CONFIG_1962HZ] = { 37, 1376,  4 },
 };
 
+// Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
+// actual cutoff differs slightly from those of the 42688P
+static aafConfig_t aafLUT42605[AAF_CONFIG_COUNT] = {  // see table in section 5.3
+    [AAF_CONFIG_258HZ]  = { 21,  440,  6 }, // actually 249 Hz
+    [AAF_CONFIG_536HZ]  = { 39, 1536,  4 }, // actually 524 Hz
+    [AAF_CONFIG_997HZ]  = { 63, 3968,  3 }, // actually 995 Hz
+    [AAF_CONFIG_1962HZ] = { 63, 3968,  3 }, // 995 Hz is the max cutoff on the 42605
+};
+
 uint8_t icm426xxSpiDetect(const extDevice_t *dev)
 {
     spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00);
@@ -191,7 +213,24 @@ bool icm426xxSpiAccDetect(accDev_t *acc)
     return true;
 }
 
-static aafConfig_t getGyroAafConfig(void);
+static aafConfig_t getGyroAafConfig(const mpuSensor_e, const aafConfig_e);
+
+static void turnGyroAccOff(const extDevice_t *dev)
+{
+    spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_GYRO_ACCEL_MODE_OFF);
+}
+
+// Turn on gyro and acc on in Low Noise mode
+static void turnGyroAccOn(const extDevice_t *dev)
+{
+    spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF | ICM426XX_PWR_MGMT0_ACCEL_MODE_LN | ICM426XX_PWR_MGMT0_GYRO_MODE_LN);
+    delay(1);
+}
+
+static void setUserBank(const extDevice_t *dev, const uint8_t user_bank)
+{
+    spiWriteReg(dev, ICM426XX_RA_REG_BANK_SEL, user_bank & 7);
+}
 
 void icm426xxGyroInit(gyroDev_t *gyro)
 {
@@ -203,42 +242,31 @@ void icm426xxGyroInit(gyroDev_t *gyro)
     gyro->accDataReg = ICM426XX_RA_ACCEL_DATA_X1;
     gyro->gyroDataReg = ICM426XX_RA_GYRO_DATA_X1;
 
-    spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF | ICM426XX_PWR_MGMT0_ACCEL_MODE_LN | ICM426XX_PWR_MGMT0_GYRO_MODE_LN);
-    delay(15);
-
-    // Get desired output data rate
-    uint8_t odrConfig;
-    const unsigned decim = llog2(gyro->mpuDividerDrops + 1);
-    if (gyro->gyroRateKHz && decim < ODR_CONFIG_COUNT) {
-        odrConfig = odrLUT[decim];
-    } else {
-        odrConfig = odrLUT[ODR_CONFIG_1K];
-        gyro->gyroRateKHz = GYRO_RATE_1_kHz;
-    }
-
-    STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
-    spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (odrConfig & 0x0F));
-    delay(15);
-
-    STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
-    spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (odrConfig & 0x0F));
-    delay(15);
+    // Turn off ACC and GYRO so they can be configured
+    // See section 12.9 in ICM-42688-P datasheet v1.7
+    setUserBank(dev, ICM426XX_BANK_SELECT0);
+    turnGyroAccOff(dev);
 
     // Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
-    aafConfig_t aafConfig = getGyroAafConfig();
+    const mpuSensor_e gyroModel = gyro->mpuDetectionResult.sensor;
+    aafConfig_t aafConfig = getGyroAafConfig(gyroModel, gyroConfig()->gyro_hardware_lpf);
+    setUserBank(dev, ICM426XX_BANK_SELECT1);
     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig.delt);
     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC4, aafConfig.deltSqr & 0xFF);
     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
 
     // Configure acc Anti-Alias Filter for 1kHz sample rate (see tasks.c)
-    aafConfig = aafLUT[AAF_CONFIG_258HZ];
+    aafConfig = getGyroAafConfig(gyroModel, AAF_CONFIG_258HZ);
+    setUserBank(dev, ICM426XX_BANK_SELECT2);
     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig.delt << 1);
     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, aafConfig.deltSqr & 0xFF);
     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC4, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
 
     // Configure gyro and acc UI Filters
+    setUserBank(dev, ICM426XX_BANK_SELECT0);
     spiWriteReg(dev, ICM426XX_RA_GYRO_ACCEL_CONFIG0, ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY);
 
+    // Configure interrupt pin
     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG, ICM426XX_INT1_MODE_PULSED | ICM426XX_INT1_DRIVE_CIRCUIT_PP | ICM426XX_INT1_POLARITY_ACTIVE_HIGH);
     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG0, ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR);
 
@@ -250,6 +278,35 @@ void icm426xxGyroInit(gyroDev_t *gyro)
     intConfig1Value |= (ICM426XX_INT_TPULSE_DURATION_8 | ICM426XX_INT_TDEASSERT_DISABLED);
 
     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG1, intConfig1Value);
+
+    // Disable AFSR to prevent stalls in gyro output
+    // ICM426XX_INTF_CONFIG1 located in user bank 0
+    uint8_t intfConfig1Value = spiReadRegMsk(dev, ICM426XX_INTF_CONFIG1);
+
+    intfConfig1Value &= ~ICM426XX_INTF_CONFIG1_AFSR_MASK;
+    intfConfig1Value |= ICM426XX_INTF_CONFIG1_AFSR_DISABLE;
+    spiWriteReg(dev, ICM426XX_INTF_CONFIG1, intfConfig1Value);
+
+    // Turn on gyro and acc on again so ODR and FSR can be configured
+    turnGyroAccOn(dev);
+
+    // Get desired output data rate
+    uint8_t odrConfig;
+    const unsigned decim = llog2(gyro->mpuDividerDrops + 1);
+    if (gyro->gyroRateKHz && decim < ODR_CONFIG_COUNT) {
+        odrConfig = odrLUT[decim];
+    } else {
+        odrConfig = odrLUT[ODR_CONFIG_1K];
+        gyro->gyroRateKHz = GYRO_RATE_1_kHz;
+    }
+
+    STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
+    spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (odrConfig & 0x0F));
+    delay(15);
+
+    STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
+    spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (odrConfig & 0x0F));
+    delay(15);
 }
 
 bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
@@ -271,21 +328,37 @@ bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
     return true;
 }
 
-static aafConfig_t getGyroAafConfig(void)
+static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig_e config)
 {
-    switch (gyroConfig()->gyro_hardware_lpf) {
-    case GYRO_HARDWARE_LPF_NORMAL:
-        return aafLUT[AAF_CONFIG_258HZ];
-    case GYRO_HARDWARE_LPF_OPTION_1:
-        return aafLUT[AAF_CONFIG_536HZ];
-    case GYRO_HARDWARE_LPF_OPTION_2:
-        return aafLUT[AAF_CONFIG_997HZ];
+    switch (gyroModel){
+    case ICM_42605_SPI:
+        switch (config) {
+        case GYRO_HARDWARE_LPF_NORMAL:
+            return aafLUT42605[AAF_CONFIG_258HZ];
+        case GYRO_HARDWARE_LPF_OPTION_1:
+            return aafLUT42605[AAF_CONFIG_536HZ];
+        case GYRO_HARDWARE_LPF_OPTION_2:
+            return aafLUT42605[AAF_CONFIG_997HZ];
+        default:
+            return aafLUT42605[AAF_CONFIG_258HZ];
+        }
+
+    case ICM_42688P_SPI:
+    default:
+        switch (config) {
+        case GYRO_HARDWARE_LPF_NORMAL:
+            return aafLUT42688[AAF_CONFIG_258HZ];
+        case GYRO_HARDWARE_LPF_OPTION_1:
+            return aafLUT42688[AAF_CONFIG_536HZ];
+        case GYRO_HARDWARE_LPF_OPTION_2:
+            return aafLUT42688[AAF_CONFIG_997HZ];
 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
-    case GYRO_HARDWARE_LPF_EXPERIMENTAL:
-        return aafLUT[AAF_CONFIG_1962HZ];
+        case GYRO_HARDWARE_LPF_EXPERIMENTAL:
+            return aafLUT42688[AAF_CONFIG_1962HZ];
 #endif
-    default:
-        return aafLUT[AAF_CONFIG_258HZ];
+        default:
+            return aafLUT42688[AAF_CONFIG_258HZ];
+        }
     }
 }
 
diff --git a/src/main/drivers/adc_stm32h7xx.c b/src/main/drivers/adc_stm32h7xx.c
index 0069859a961..b9dac057778 100644
--- a/src/main/drivers/adc_stm32h7xx.c
+++ b/src/main/drivers/adc_stm32h7xx.c
@@ -272,7 +272,12 @@ void adcInitCalibrationValues(void)
 // Need this separate from the main adcValue[] array, because channels are numbered
 // by ADC instance order that is different from ADC_xxx numbering.
 
-static volatile DMA_RAM uint16_t adcConversionBuffer[ADC_CHANNEL_COUNT] __attribute__((aligned(32)));
+#define ADC_BUF_LENGTH ADC_CHANNEL_COUNT
+#define ADC_BUF_BYTES (ADC_BUF_LENGTH * sizeof(uint16_t))
+#define ADC_BUF_CACHE_ALIGN_BYTES  ((ADC_BUF_BYTES + 0x20) & ~0x1f)
+#define ADC_BUF_CACHE_ALIGN_LENGTH (ADC_BUF_CACHE_ALIGN_BYTES / sizeof(uint16_t))
+
+static volatile DMA_RAM uint16_t adcConversionBuffer[ADC_BUF_CACHE_ALIGN_LENGTH] __attribute__((aligned(32)));
 
 void adcInit(const adcConfig_t *config)
 {
@@ -535,8 +540,7 @@ void adcInit(const adcConfig_t *config)
 void adcGetChannelValues(void)
 {
     // Transfer values in conversion buffer into adcValues[]
-    // Cache coherency should be maintained by MPU facility
-
+    SCB_InvalidateDCache_by_Addr((uint32_t*)adcConversionBuffer, ADC_BUF_CACHE_ALIGN_BYTES);
     for (int i = 0; i < ADC_CHANNEL_INTERNAL_FIRST_ID; i++) {
         if (adcOperatingConfig[i].enabled) {
             adcValues[adcOperatingConfig[i].dmaIndex] = adcConversionBuffer[adcOperatingConfig[i].dmaIndex];
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 2443246d115..f3f1e02e0d3 100644
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -42,6 +42,7 @@ typedef enum {
     BUS_ABORT
 } busStatus_e;
 
+struct extDevice_s;
 
 // Bus interface, independent of connected device
 typedef struct busDevice_s {
@@ -74,6 +75,7 @@ typedef struct busDevice_s {
 #endif
 #endif // UNIT_TEST
     volatile struct busSegment_s* volatile curSegment;
+    volatile struct extDevice_s *csLockDevice;
     bool initSegment;
 } busDevice_t;
 
diff --git a/src/main/drivers/bus_octospi.c b/src/main/drivers/bus_octospi.c
new file mode 100644
index 00000000000..d51c59397a0
--- /dev/null
+++ b/src/main/drivers/bus_octospi.c
@@ -0,0 +1,119 @@
+/*
+ * 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
+ */
+
+/*
+ * OctoSPI support.
+ *
+ * Some STM32H7 MCUs support 2 OCTOSPI peripherals, each with up to 2 flash chips, using 2/4/8 IO lines each. Small MCU packages only have one instance.
+ * Additionally there is an OCTOSPIM peripheral which maps OCTOSPI1/2 peripherals to IO pins and can perform arbitration.
+ *
+ * Initial implementation is focused on supporting memory-mapped flash chips connected to an OCTOSPI peripheral that is already initialised by a bootloader.
+ *
+ * As such the following things are NOT supported:
+ * * Configuration of IO pins.
+ * * Clock configuration.
+ * * User-configuration.
+ * * OCTOSPIM configuration.
+ * * OCTOSPI2.
+ *
+ * Should the firmware need to know about the pins used by OCTOSPI then code can be written to determine this from the registers of the OCTOSPIM and OCTOSPI1/2 peripherals.
+ *
+ * Implementation notes:
+ * It's not possible to use the HAL libraries without modifying them and maintaining the internal state of the HAL structures.
+ * The HAL libraries were not designed to support the use-case of a bootloader configuring the flash in memory mapped mode and then
+ * having a second piece of software (this firmware) also use the flash.  Furthermore many HAL methods were not designed to run with
+ * interrupts disabled which is necessary as other ISRs in this firmware will be running from external flash and must be disabled.
+ * See HAL_OSPI_Abort, OSPI_WaitFlagStateUntilTimeout, etc.
+ * All code that is executed when memory mapped mode is disabled needs to run from RAM, this would also involve modification of the HAL
+ * libraries.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef USE_OCTOSPI
+
+#include "bus_octospi.h"
+#include "bus_octospi_impl.h"
+
+octoSpiDevice_t octoSpiDevice[OCTOSPIDEV_COUNT] = { 0 };
+
+MMFLASH_CODE_NOINLINE OCTOSPIDevice octoSpiDeviceByInstance(OCTOSPI_TypeDef *instance)
+{
+#ifdef USE_OCTOSPI_DEVICE_1
+    if (instance == OCTOSPI1) {
+        return OCTOSPIDEV_1;
+    }
+#endif
+
+    return OCTOSPIINVALID;
+}
+
+OCTOSPI_TypeDef *octoSpiInstanceByDevice(OCTOSPIDevice device)
+{
+    if (device == OCTOSPIINVALID || device >= OCTOSPIDEV_COUNT) {
+        return NULL;
+    }
+
+    return octoSpiDevice[device].dev;
+}
+
+const octoSpiHardware_t octoSpiHardware[] = {
+#if defined(STM32H730xx) || defined(STM32H723xx)
+    {
+        .device = OCTOSPIDEV_1,
+        .reg = OCTOSPI1,
+    }
+#else
+#error MCU not supported.
+#endif
+};
+
+bool octoSpiInit(OCTOSPIDevice device)
+{
+    for (size_t hwindex = 0; hwindex < ARRAYLEN(octoSpiHardware); hwindex++) {
+        const octoSpiHardware_t *hw = &octoSpiHardware[hwindex];
+
+        OCTOSPIDevice device = hw->device;
+        octoSpiDevice_t *pDev = &octoSpiDevice[device];
+
+        pDev->dev = hw->reg;
+    }
+
+    switch (device) {
+    case OCTOSPIINVALID:
+        return false;
+    case OCTOSPIDEV_1:
+#ifdef USE_OCTOSPI_DEVICE_1
+        octoSpiInitDevice(OCTOSPIDEV_1);
+        return true;
+#else
+        break;
+#endif
+    }
+    return false;
+}
+
+#endif
diff --git a/src/main/drivers/bus_octospi.h b/src/main/drivers/bus_octospi.h
new file mode 100644
index 00000000000..14178bd7934
--- /dev/null
+++ b/src/main/drivers/bus_octospi.h
@@ -0,0 +1,62 @@
+/*
+ * 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
+
+#ifdef USE_OCTOSPI
+
+typedef enum OCTOSPIDevice {
+    OCTOSPIINVALID = -1,
+    OCTOSPIDEV_1   = 0,
+} OCTOSPIDevice;
+
+#define OCTOSPIDEV_COUNT 1
+
+// Macros to convert between configuration ids and device ids.
+#define OCTOSPI_CFG_TO_DEV(x)   ((x) - 1)
+#define OCTOSPI_DEV_TO_CFG(x)   ((x) + 1)
+
+#if !defined(STM32H7)
+#error OctoSPI unsupported on this MCU
+#endif
+
+OCTOSPIDevice octoSpiDeviceByInstance(OCTOSPI_TypeDef *instance);
+OCTOSPI_TypeDef *octoSpiInstanceByDevice(OCTOSPIDevice device);
+
+
+bool octoSpiInit(OCTOSPIDevice device);
+bool octoSpiReceive1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length);
+bool octoSpiReceive4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length);
+bool octoSpiTransmit1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length);
+
+bool octoSpiReceiveWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length);
+bool octoSpiReceiveWithAddress4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length);
+bool octoSpiTransmitWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length);
+bool octoSpiTransmitWithAddress4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length);
+
+bool octoSpiInstructionWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize);
+
+
+void octoSpiDisableMemoryMappedMode(OCTOSPI_TypeDef *instance);
+void octoSpiEnableMemoryMappedMode(OCTOSPI_TypeDef *instance);
+
+#endif
diff --git a/src/main/drivers/bus_octospi_impl.h b/src/main/drivers/bus_octospi_impl.h
new file mode 100644
index 00000000000..3a229892383
--- /dev/null
+++ b/src/main/drivers/bus_octospi_impl.h
@@ -0,0 +1,40 @@
+/*
+ * 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
+
+#ifdef USE_OCTOSPI
+
+typedef struct octoSpiHardware_s {
+    OCTOSPIDevice device;
+    OCTOSPI_TypeDef *reg;
+} octoSpiHardware_t;
+
+typedef struct OCTOSPIDevice_s {
+    OCTOSPI_TypeDef *dev;
+} octoSpiDevice_t;
+
+extern octoSpiDevice_t octoSpiDevice[OCTOSPIDEV_COUNT];
+
+void octoSpiInitDevice(OCTOSPIDevice device);
+
+#endif
diff --git a/src/main/drivers/bus_octospi_stm32h7xx.c b/src/main/drivers/bus_octospi_stm32h7xx.c
new file mode 100644
index 00000000000..f45aaddb441
--- /dev/null
+++ b/src/main/drivers/bus_octospi_stm32h7xx.c
@@ -0,0 +1,853 @@
+/*
+ * 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_OCTOSPI
+
+#include "drivers/system.h"
+
+#include "drivers/bus_octospi.h"
+#include "drivers/bus_octospi_impl.h"
+
+#if !(defined(STM32H730xx) || defined(STM32H723xx))
+#error MCU not supported.
+#endif
+
+#define OCTOSPI_INTERFACE_COUNT         1
+
+#define OSPI_FUNCTIONAL_MODE_INDIRECT_WRITE ((uint32_t)0x00000000)
+#define OSPI_FUNCTIONAL_MODE_INDIRECT_READ  ((uint32_t)OCTOSPI_CR_FMODE_0)
+#define OSPI_FUNCTIONAL_MODE_AUTO_POLLING   ((uint32_t)OCTOSPI_CR_FMODE_1)
+#define OSPI_FUNCTIONAL_MODE_MEMORY_MAPPED  ((uint32_t)OCTOSPI_CR_FMODE)
+
+#define OSPI_DHQC_DISABLE                ((uint32_t)0x00000000U)
+#define OSPI_DHQC_ENABLE                 ((uint32_t)OCTOSPI_TCR_DHQC)
+
+#define OSPI_OPTYPE_COMMON_CFG           ((uint32_t)0x00000000U)
+
+#define OSPI_OPTYPE_READ_CFG             ((uint32_t)0x00000001U)
+#define OSPI_OPTYPE_WRITE_CFG            ((uint32_t)0x00000002U)
+#define OSPI_OPTYPE_WRAP_CFG             ((uint32_t)0x00000003U)
+
+#define OSPI_INSTRUCTION_NONE            ((uint32_t)0x00000000U)
+#define OSPI_INSTRUCTION_1_LINE          ((uint32_t)OCTOSPI_CCR_IMODE_0)
+#define OSPI_INSTRUCTION_2_LINES         ((uint32_t)OCTOSPI_CCR_IMODE_1)
+#define OSPI_INSTRUCTION_4_LINES         ((uint32_t)(OCTOSPI_CCR_IMODE_0 | OCTOSPI_CCR_IMODE_1))
+#define OSPI_INSTRUCTION_8_LINES         ((uint32_t)OCTOSPI_CCR_IMODE_2)
+
+#define OSPI_INSTRUCTION_8_BITS          ((uint32_t)0x00000000U)
+#define OSPI_INSTRUCTION_16_BITS         ((uint32_t)OCTOSPI_CCR_ISIZE_0)
+#define OSPI_INSTRUCTION_24_BITS         ((uint32_t)OCTOSPI_CCR_ISIZE_1)
+#define OSPI_INSTRUCTION_32_BITS         ((uint32_t)OCTOSPI_CCR_ISIZE)
+
+#define OSPI_INSTRUCTION_DTR_DISABLE     ((uint32_t)0x00000000U)
+#define OSPI_INSTRUCTION_DTR_ENABLE      ((uint32_t)OCTOSPI_CCR_IDTR)
+
+#define OSPI_ADDRESS_NONE                ((uint32_t)0x00000000U)                                         /*!< No address               */
+#define OSPI_ADDRESS_1_LINE              ((uint32_t)OCTOSPI_CCR_ADMODE_0)                                /*!< Address on a single line */
+#define OSPI_ADDRESS_2_LINES             ((uint32_t)OCTOSPI_CCR_ADMODE_1)                                /*!< Address on two lines     */
+#define OSPI_ADDRESS_4_LINES             ((uint32_t)(OCTOSPI_CCR_ADMODE_0 | OCTOSPI_CCR_ADMODE_1))       /*!< Address on four lines    */
+#define OSPI_ADDRESS_8_LINES             ((uint32_t)OCTOSPI_CCR_ADMODE_2)                                /*!< Address on eight lines   */
+
+#define OSPI_ADDRESS_8_BITS              ((uint32_t)0x00000000U)                                         /*!< 8-bit address  */
+#define OSPI_ADDRESS_16_BITS             ((uint32_t)OCTOSPI_CCR_ADSIZE_0)                                /*!< 16-bit address */
+#define OSPI_ADDRESS_24_BITS             ((uint32_t)OCTOSPI_CCR_ADSIZE_1)                                /*!< 24-bit address */
+#define OSPI_ADDRESS_32_BITS             ((uint32_t)OCTOSPI_CCR_ADSIZE)                                  /*!< 32-bit address */
+
+#define OSPI_ADDRESS_DTR_DISABLE         ((uint32_t)0x00000000U)                                         /*!< DTR mode disabled for address phase */
+#define OSPI_ADDRESS_DTR_ENABLE          ((uint32_t)OCTOSPI_CCR_ADDTR)
+
+#define OSPI_DATA_NONE                   ((uint32_t)0x00000000U)
+#define OSPI_DATA_1_LINE                 ((uint32_t)OCTOSPI_CCR_DMODE_0)
+#define OSPI_DATA_2_LINES                ((uint32_t)OCTOSPI_CCR_DMODE_1)
+#define OSPI_DATA_4_LINES                ((uint32_t)(OCTOSPI_CCR_DMODE_0 | OCTOSPI_CCR_DMODE_1))
+#define OSPI_DATA_8_LINES                ((uint32_t)OCTOSPI_CCR_DMODE_2)
+
+#define OSPI_DATA_DTR_DISABLE            ((uint32_t)0x00000000U)
+#define OSPI_DATA_DTR_ENABLE             ((uint32_t)OCTOSPI_CCR_DDTR)
+
+#define OSPI_ALTERNATE_BYTES_NONE        ((uint32_t)0x00000000U)
+#define OSPI_ALTERNATE_BYTES_1_LINE      ((uint32_t)OCTOSPI_CCR_ABMODE_0)
+#define OSPI_ALTERNATE_BYTES_2_LINES     ((uint32_t)OCTOSPI_CCR_ABMODE_1)
+#define OSPI_ALTERNATE_BYTES_4_LINES     ((uint32_t)(OCTOSPI_CCR_ABMODE_0 | OCTOSPI_CCR_ABMODE_1))
+#define OSPI_ALTERNATE_BYTES_8_LINES     ((uint32_t)OCTOSPI_CCR_ABMODE_2)
+
+#define OSPI_ALTERNATE_BYTES_8_BITS      ((uint32_t)0x00000000U)
+#define OSPI_ALTERNATE_BYTES_16_BITS     ((uint32_t)OCTOSPI_CCR_ABSIZE_0)
+#define OSPI_ALTERNATE_BYTES_24_BITS     ((uint32_t)OCTOSPI_CCR_ABSIZE_1)
+#define OSPI_ALTERNATE_BYTES_32_BITS     ((uint32_t)OCTOSPI_CCR_ABSIZE)
+
+#define OSPI_ALTERNATE_BYTES_DTR_DISABLE ((uint32_t)0x00000000U)
+#define OSPI_ALTERNATE_BYTES_DTR_ENABLE  ((uint32_t)OCTOSPI_CCR_ABDTR)
+
+#define OSPI_DQS_DISABLE                 ((uint32_t)0x00000000U)
+#define OSPI_DQS_ENABLE                  ((uint32_t)OCTOSPI_CCR_DQSE)
+
+#define OSPI_SIOO_INST_EVERY_CMD         ((uint32_t)0x00000000U)
+#define OSPI_SIOO_INST_ONLY_FIRST_CMD    ((uint32_t)OCTOSPI_CCR_SIOO)
+
+MMFLASH_CODE_NOINLINE static void Error_Handler(void) {
+    while (1) {
+        NOOP;
+    }
+}
+
+
+#define __OSPI_GET_FLAG(__INSTANCE__, __FLAG__)           ((READ_BIT((__INSTANCE__)->SR, (__FLAG__)) != 0U) ? SET : RESET)
+#define __OSPI_CLEAR_FLAG(__INSTANCE__, __FLAG__)           WRITE_REG((__INSTANCE__)->FCR, (__FLAG__))
+#define __OSPI_ENABLE(__INSTANCE__)                       SET_BIT((__INSTANCE__)->CR, OCTOSPI_CR_EN)
+#define __OSPI_DISABLE(__INSTANCE__)                      CLEAR_BIT((__INSTANCE__)->CR, OCTOSPI_CR_EN)
+#define __OSPI_IS_ENABLED(__INSTANCE__)                   (READ_BIT((__INSTANCE__)->CR, OCTOSPI_CR_EN) != 0U)
+
+MMFLASH_CODE_NOINLINE static void octoSpiAbort(OCTOSPI_TypeDef *instance)
+{
+    SET_BIT(instance->CR, OCTOSPI_CR_ABORT);
+}
+
+MMFLASH_CODE_NOINLINE static void octoSpiWaitStatusFlags(OCTOSPI_TypeDef *instance, uint32_t mask, FlagStatus flagStatus)
+{
+    uint32_t regval;
+
+    switch (flagStatus) {
+    case SET:
+        while (!((regval = READ_REG(instance->SR)) & mask))
+            {}
+        break;
+    case RESET:
+        while (((regval = READ_REG(instance->SR)) & mask))
+            {}
+        break;
+    }
+}
+
+
+typedef struct {
+    uint32_t OperationType;
+
+    uint32_t Instruction;
+    uint32_t InstructionMode;
+    uint32_t InstructionSize;
+    uint32_t InstructionDtrMode;
+
+    uint32_t Address;
+    uint32_t AddressMode;
+    uint32_t AddressSize;
+    uint32_t AddressDtrMode;
+
+    uint32_t AlternateBytes;
+    uint32_t AlternateBytesMode;
+    uint32_t AlternateBytesSize;
+    uint32_t AlternateBytesDtrMode;
+
+    uint32_t DummyCycles; // 0-31
+
+    uint32_t DataMode;
+    uint32_t DataDtrMode;
+    uint32_t NbData;
+
+    uint32_t DQSMode;
+    uint32_t SIOOMode;
+} OSPI_Command_t;
+
+// TODO rename cmd to command
+MMFLASH_CODE_NOINLINE static ErrorStatus octoSpiConfigureCommand(OCTOSPI_TypeDef *instance, OSPI_Command_t *cmd)
+{
+    ErrorStatus status = SUCCESS;
+
+    MODIFY_REG(instance->CR, OCTOSPI_CR_FMODE, 0U);
+
+    instance->CCR = (cmd->DQSMode | cmd->SIOOMode);
+
+    if (cmd->AlternateBytesMode != OSPI_ALTERNATE_BYTES_NONE)
+    {
+        instance->ABR = cmd->AlternateBytes;
+
+        MODIFY_REG(
+            instance->ABR,
+            (OCTOSPI_CCR_ABMODE | OCTOSPI_CCR_ABDTR | OCTOSPI_CCR_ABSIZE),
+            (cmd->AlternateBytesMode | cmd->AlternateBytesDtrMode | cmd->AlternateBytesSize)
+        );
+    }
+
+    MODIFY_REG(instance->TCR, OCTOSPI_TCR_DCYC, cmd->DummyCycles);
+
+    if (cmd->DataMode != OSPI_DATA_NONE)
+    {
+      if (cmd->OperationType == OSPI_OPTYPE_COMMON_CFG)
+      {
+        instance->DLR = (cmd->NbData - 1U);
+      }
+    }
+
+
+    if (cmd->InstructionMode != OSPI_INSTRUCTION_NONE)
+    {
+      if (cmd->AddressMode != OSPI_ADDRESS_NONE)
+      {
+        if (cmd->DataMode != OSPI_DATA_NONE)
+        {
+          // instruction, address and data
+
+          MODIFY_REG(instance->CCR, (OCTOSPI_CCR_IMODE  | OCTOSPI_CCR_IDTR  | OCTOSPI_CCR_ISIZE  |
+                                  OCTOSPI_CCR_ADMODE | OCTOSPI_CCR_ADDTR | OCTOSPI_CCR_ADSIZE |
+                                  OCTOSPI_CCR_DMODE  | OCTOSPI_CCR_DDTR),
+                                 (cmd->InstructionMode | cmd->InstructionDtrMode | cmd->InstructionSize |
+                                  cmd->AddressMode     | cmd->AddressDtrMode     | cmd->AddressSize     |
+                                  cmd->DataMode        | cmd->DataDtrMode));
+        }
+        else
+        {
+          // instruction and address
+
+          MODIFY_REG(instance->CCR, (OCTOSPI_CCR_IMODE  | OCTOSPI_CCR_IDTR  | OCTOSPI_CCR_ISIZE  |
+                                  OCTOSPI_CCR_ADMODE | OCTOSPI_CCR_ADDTR | OCTOSPI_CCR_ADSIZE),
+                                 (cmd->InstructionMode | cmd->InstructionDtrMode | cmd->InstructionSize |
+                                  cmd->AddressMode     | cmd->AddressDtrMode     | cmd->AddressSize));
+
+          // DHQC bit is linked with DDTR
+          if (((instance->TCR & OCTOSPI_TCR_DHQC_Msk) == OSPI_DHQC_ENABLE) &&
+              (cmd->InstructionDtrMode == OSPI_INSTRUCTION_DTR_ENABLE))
+          {
+            MODIFY_REG(instance->CCR, OCTOSPI_CCR_DDTR, OSPI_DATA_DTR_ENABLE);
+          }
+        }
+
+        instance->IR = cmd->Instruction;
+
+        instance->AR = cmd->Address;
+      }
+      else
+      {
+        if (cmd->DataMode != OSPI_DATA_NONE)
+        {
+          // instruction and data
+
+          MODIFY_REG(instance->CCR, (OCTOSPI_CCR_IMODE | OCTOSPI_CCR_IDTR | OCTOSPI_CCR_ISIZE |
+                                  OCTOSPI_CCR_DMODE | OCTOSPI_CCR_DDTR),
+                                 (cmd->InstructionMode | cmd->InstructionDtrMode | cmd->InstructionSize |
+                                  cmd->DataMode        | cmd->DataDtrMode));
+        }
+        else
+        {
+          // instruction only
+
+          MODIFY_REG(instance->CCR, (OCTOSPI_CCR_IMODE | OCTOSPI_CCR_IDTR | OCTOSPI_CCR_ISIZE),
+                                 (cmd->InstructionMode | cmd->InstructionDtrMode | cmd->InstructionSize));
+
+          // DHQC bit is linked with DDTR
+          if (((instance->TCR & OCTOSPI_TCR_DHQC_Msk) == OSPI_DHQC_ENABLE) &&
+              (cmd->InstructionDtrMode == OSPI_INSTRUCTION_DTR_ENABLE))
+          {
+            MODIFY_REG(instance->CCR, OCTOSPI_CCR_DDTR, OSPI_DATA_DTR_ENABLE);
+          }
+        }
+
+        instance->IR = cmd->Instruction;
+
+      }
+    }
+    else
+    {
+      if (cmd->AddressMode != OSPI_ADDRESS_NONE)
+      {
+        if (cmd->DataMode != OSPI_DATA_NONE)
+        {
+          // address and data
+
+          MODIFY_REG(instance->CCR, (OCTOSPI_CCR_ADMODE | OCTOSPI_CCR_ADDTR | OCTOSPI_CCR_ADSIZE |
+                                  OCTOSPI_CCR_DMODE  | OCTOSPI_CCR_DDTR),
+                                 (cmd->AddressMode | cmd->AddressDtrMode | cmd->AddressSize     |
+                                  cmd->DataMode    | cmd->DataDtrMode));
+        }
+        else
+        {
+          // address only
+
+          MODIFY_REG(instance->CCR, (OCTOSPI_CCR_ADMODE | OCTOSPI_CCR_ADDTR | OCTOSPI_CCR_ADSIZE),
+                                 (cmd->AddressMode | cmd->AddressDtrMode | cmd->AddressSize));
+        }
+
+        instance->AR = cmd->Address;
+      }
+      else
+      {
+        // no instruction, no address
+        status = ERROR;
+      }
+    }
+
+    return status;
+}
+
+MMFLASH_CODE_NOINLINE ErrorStatus octoSpiCommand(OCTOSPI_TypeDef *instance, OSPI_Command_t *cmd)
+{
+    octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET);
+
+    ErrorStatus status = octoSpiConfigureCommand(instance, cmd);
+    if (status == SUCCESS) {
+        if (cmd->DataMode == OSPI_DATA_NONE)
+        {
+            // transfer is already started, wait now.
+            octoSpiWaitStatusFlags(instance, OCTOSPI_SR_TCF, SET);
+            __OSPI_CLEAR_FLAG(instance, OCTOSPI_SR_TCF);
+        }
+    }
+
+    return status;
+}
+
+/*
+ * Transmit
+ *
+ * Call optoSpiCommand first to configure the transaction stages.
+ */
+MMFLASH_CODE_NOINLINE ErrorStatus octoSpiTransmit(OCTOSPI_TypeDef *instance, uint8_t *data)
+{
+    if (data == NULL) {
+        return ERROR;
+    }
+
+
+    __IO uint32_t              XferCount = READ_REG(instance->DLR) + 1U;
+    uint8_t                    *pBuffPtr = data;
+
+    MODIFY_REG(instance->CR, OCTOSPI_CR_FMODE, OSPI_FUNCTIONAL_MODE_INDIRECT_WRITE);
+
+    __IO uint32_t *data_reg = &instance->DR;
+    do
+    {
+      octoSpiWaitStatusFlags(instance, OCTOSPI_SR_FTF, SET);
+
+      *((__IO uint8_t *)data_reg) = *pBuffPtr;
+      pBuffPtr++;
+      XferCount--;
+    } while (XferCount > 0U);
+
+    octoSpiWaitStatusFlags(instance, OCTOSPI_SR_TCF, SET);
+    __OSPI_CLEAR_FLAG(instance, OCTOSPI_SR_TCF);
+
+    return SUCCESS;
+}
+
+/*
+ * Receive
+ *
+ * Call optoSpiCommand first to configure the transaction stages.
+ */
+MMFLASH_CODE_NOINLINE ErrorStatus octoSpiReceive(OCTOSPI_TypeDef *instance, uint8_t *data)
+{
+    if (data == NULL) {
+        return ERROR;
+    }
+
+    __IO uint32_t              XferCount = READ_REG(instance->DLR) + 1U;
+    uint8_t                    *pBuffPtr = data;
+
+    MODIFY_REG(instance->CR, OCTOSPI_CR_FMODE, OSPI_FUNCTIONAL_MODE_INDIRECT_READ);
+
+    uint32_t addr_reg = instance->AR;
+    uint32_t ir_reg = instance->IR;
+
+    // Trigger the transfer by re-writing the address or instruction register
+    if (READ_BIT(instance->CCR, OCTOSPI_CCR_ADMODE) != OSPI_ADDRESS_NONE)
+    {
+      WRITE_REG(instance->AR, addr_reg);
+    }
+    else
+    {
+      WRITE_REG(instance->IR, ir_reg);
+    }
+
+    __IO uint32_t *data_reg = &instance->DR;
+
+    do
+    {
+      octoSpiWaitStatusFlags(instance, OCTOSPI_SR_FTF | OCTOSPI_SR_TCF, SET);
+
+      *pBuffPtr = *((__IO uint8_t *)data_reg);
+      pBuffPtr++;
+      XferCount--;
+    } while(XferCount > 0U);
+
+    octoSpiWaitStatusFlags(instance, OCTOSPI_SR_TCF, SET);
+    __OSPI_CLEAR_FLAG(instance, OCTOSPI_SR_TCF);
+
+    return SUCCESS;
+}
+
+typedef struct
+{
+    // CR register contains the all-important FMODE bits.
+    uint32_t CR;
+
+    // flash chip specific configuration set by the bootloader
+    uint32_t CCR;
+    uint32_t TCR;
+    uint32_t IR;
+    uint32_t ABR;
+    // address register - no meaning.
+    // data length register no meaning.
+
+} octoSpiMemoryMappedModeConfigurationRegisterBackup_t;
+
+octoSpiMemoryMappedModeConfigurationRegisterBackup_t ospiMMMCRBackups[OCTOSPI_INTERFACE_COUNT];
+
+void octoSpiBackupMemoryMappedModeConfiguration(OCTOSPI_TypeDef *instance)
+{
+    OCTOSPIDevice device = octoSpiDeviceByInstance(instance);
+    if (device == OCTOSPIINVALID) {
+        return;
+    }
+
+    octoSpiMemoryMappedModeConfigurationRegisterBackup_t *ospiMMMCRBackup = &ospiMMMCRBackups[device];
+
+    // backup all the registers used by memory mapped mode that:
+    // a) the bootloader configured.
+    // b) that other code in this implementation may have modified when memory mapped mode is disabled.
+
+    ospiMMMCRBackup->CR = instance->CR;
+    ospiMMMCRBackup->IR = instance->IR;
+    ospiMMMCRBackup->CCR = instance->CCR;
+    ospiMMMCRBackup->TCR = instance->TCR;
+    ospiMMMCRBackup->ABR = instance->ABR;
+}
+
+MMFLASH_CODE_NOINLINE void octoSpiRestoreMemoryMappedModeConfiguration(OCTOSPI_TypeDef *instance)
+{
+    OCTOSPIDevice device = octoSpiDeviceByInstance(instance);
+    if (device == OCTOSPIINVALID) {
+        return;
+    }
+
+    octoSpiMemoryMappedModeConfigurationRegisterBackup_t *ospiMMMCRBackup = &ospiMMMCRBackups[device];
+
+    octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET);
+
+    instance->ABR = ospiMMMCRBackup->ABR;
+    instance->TCR = ospiMMMCRBackup->TCR;
+
+    instance->DLR = 0; // "no meaning" in MM mode.
+
+    instance->CCR = ospiMMMCRBackup->CCR;
+
+    instance->IR = ospiMMMCRBackup->IR;
+    instance->AR = 0; // "no meaning" in MM mode.
+
+    octoSpiAbort(instance);
+    octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET);
+
+    instance->CR = ospiMMMCRBackup->CR;
+}
+
+/*
+ * Disable memory mapped mode.
+ *
+ * @See octoSpiEnableMemoryMappedMode
+ * @See MMFLASH_CODE_NOINLINE
+ *
+ * Once this is called any code or data in the memory mapped region cannot be accessed.
+ * Thus, this function itself must be in RAM, and the caller's code and data should all be in RAM
+ * and this requirement continues until octoSpiEnableMemoryMappedMode is called.
+ * This applies to ISR code that runs from the memory mapped region, so likely the caller should
+ * also disable IRQs before calling this.
+ */
+MMFLASH_CODE_NOINLINE void octoSpiDisableMemoryMappedMode(OCTOSPI_TypeDef *instance)
+{
+    if (READ_BIT(OCTOSPI1->CR, OCTOSPI_CR_FMODE) != OCTOSPI_CR_FMODE) {
+        failureMode(FAILURE_DEVELOPER); // likely not booted with memory mapped mode enabled, or mismatched calls to enable/disable memory map mode.
+    }
+
+    octoSpiAbort(instance);
+    if (__OSPI_GET_FLAG(instance, OCTOSPI_SR_BUSY) == SET) {
+
+        __OSPI_DISABLE(instance);
+        octoSpiAbort(instance);
+    }
+    octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET);
+
+    uint32_t fmode = 0x0;  // b00 = indirect write, see OCTOSPI->CR->FMODE
+    MODIFY_REG(instance->CR, OCTOSPI_CR_FMODE, fmode);
+
+    uint32_t regval = READ_REG(instance->CR);
+    if ((regval & OCTOSPI_CR_FMODE) != fmode) {
+        Error_Handler();
+    }
+
+    if (!__OSPI_IS_ENABLED(instance)) {
+        __OSPI_ENABLE(instance);
+    }
+}
+
+/*
+ * Enable memory mapped mode.
+ *
+ * @See octoSpiDisableMemoryMappedMode
+ * @See MMFLASH_CODE_NOINLINE
+ */
+
+MMFLASH_CODE_NOINLINE void octoSpiEnableMemoryMappedMode(OCTOSPI_TypeDef *instance)
+{
+    octoSpiAbort(instance);
+    octoSpiWaitStatusFlags(instance, OCTOSPI_SR_BUSY, RESET);
+
+    octoSpiRestoreMemoryMappedModeConfiguration(instance);
+}
+
+MMFLASH_CODE_NOINLINE void octoSpiTestEnableDisableMemoryMappedMode(octoSpiDevice_t *octoSpi)
+{
+    OCTOSPI_TypeDef *instance = octoSpi->dev;
+
+    __disable_irq();
+    octoSpiDisableMemoryMappedMode(instance);
+    octoSpiEnableMemoryMappedMode(instance);
+    __enable_irq();
+}
+
+MMFLASH_DATA static const uint32_t octoSpi_addressSizeMap[] = {
+    OSPI_ADDRESS_8_BITS,
+    OSPI_ADDRESS_16_BITS,
+    OSPI_ADDRESS_24_BITS,
+    OSPI_ADDRESS_32_BITS
+};
+
+MMFLASH_CODE static uint32_t octoSpi_addressSizeFromValue(uint8_t addressSize)
+{
+    return octoSpi_addressSizeMap[((addressSize + 1) / 8) - 1]; // rounds to nearest OSPI_ADDRESS_* value that will hold the address.
+}
+
+
+MMFLASH_CODE_NOINLINE bool octoSpiTransmit1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, const uint8_t *out, int length)
+{
+    OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM.
+
+    cmd.OperationType      = OSPI_OPTYPE_COMMON_CFG;
+
+    cmd.Instruction        = instruction;
+    cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE;
+    cmd.InstructionMode    = OSPI_INSTRUCTION_1_LINE;
+    cmd.InstructionSize    = OSPI_INSTRUCTION_8_BITS;
+
+    cmd.AddressDtrMode     = OSPI_ADDRESS_DTR_DISABLE;
+    cmd.AddressMode        = OSPI_ADDRESS_NONE;
+    cmd.AddressSize        = OSPI_ADDRESS_32_BITS;
+
+    cmd.DummyCycles        = dummyCycles;
+
+    cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE;
+
+    cmd.DataDtrMode        = OSPI_DATA_DTR_DISABLE;
+    cmd.DataMode           = OSPI_DATA_NONE;
+    cmd.NbData             = length;
+
+    cmd.DQSMode            = OSPI_DQS_DISABLE;
+    cmd.SIOOMode           = OSPI_SIOO_INST_EVERY_CMD;
+
+    if (out) {
+        cmd.DataMode       = OSPI_DATA_1_LINE;
+    }
+
+    ErrorStatus status = octoSpiCommand(instance, &cmd);
+
+    if (status == SUCCESS && out && length > 0) {
+        status = octoSpiTransmit(instance, (uint8_t *)out);
+    }
+    return status == SUCCESS;
+}
+
+MMFLASH_CODE_NOINLINE bool octoSpiReceive1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length)
+{
+    OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM.
+
+    cmd.OperationType      = OSPI_OPTYPE_COMMON_CFG;
+
+    cmd.Instruction        = instruction;
+    cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE;
+    cmd.InstructionMode    = OSPI_INSTRUCTION_1_LINE;
+    cmd.InstructionSize    = OSPI_INSTRUCTION_8_BITS;
+
+    cmd.AddressDtrMode     = OSPI_ADDRESS_DTR_DISABLE;
+    cmd.AddressMode        = OSPI_ADDRESS_NONE;
+    cmd.AddressSize        = OSPI_ADDRESS_32_BITS;
+
+    cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE;
+
+    cmd.DummyCycles        = dummyCycles;
+
+    cmd.DataDtrMode        = OSPI_DATA_DTR_DISABLE;
+    cmd.DataMode           = OSPI_DATA_1_LINE;
+    cmd.NbData             = length;
+
+    cmd.DQSMode            = OSPI_DQS_DISABLE;
+    cmd.SIOOMode           = OSPI_SIOO_INST_EVERY_CMD;
+
+    ErrorStatus status = octoSpiCommand(instance, &cmd);
+
+    if (status == SUCCESS) {
+        status = octoSpiReceive(instance, in);
+    }
+
+    return status == SUCCESS;
+}
+
+MMFLASH_CODE_NOINLINE bool octoSpiReceive4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint8_t *in, int length)
+{
+    OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM.
+
+    cmd.OperationType      = OSPI_OPTYPE_COMMON_CFG;
+
+    cmd.Instruction        = instruction;
+    cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE;
+    cmd.InstructionMode    = OSPI_INSTRUCTION_4_LINES;
+    cmd.InstructionSize    = OSPI_INSTRUCTION_8_BITS;
+
+    cmd.AddressDtrMode     = OSPI_ADDRESS_DTR_DISABLE;
+    cmd.AddressMode        = OSPI_ADDRESS_NONE;
+    cmd.AddressSize        = OSPI_ADDRESS_32_BITS;
+
+    cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE;
+
+    cmd.DummyCycles        = dummyCycles;
+
+    cmd.DataDtrMode        = OSPI_DATA_DTR_DISABLE;
+    cmd.DataMode           = OSPI_DATA_4_LINES;
+    cmd.NbData             = length;
+
+    cmd.DQSMode            = OSPI_DQS_DISABLE;
+    cmd.SIOOMode           = OSPI_SIOO_INST_EVERY_CMD;
+
+    ErrorStatus status = octoSpiCommand(instance, &cmd);
+
+    if (status == SUCCESS) {
+        status = octoSpiReceive(instance, in);
+    }
+
+    return status == SUCCESS;
+}
+
+MMFLASH_CODE_NOINLINE bool octoSpiReceiveWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length)
+{
+    OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM.
+
+    cmd.OperationType      = OSPI_OPTYPE_COMMON_CFG;
+
+    cmd.Instruction        = instruction;
+    cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE;
+    cmd.InstructionMode    = OSPI_INSTRUCTION_1_LINE;
+    cmd.InstructionSize    = OSPI_INSTRUCTION_8_BITS;
+
+    cmd.Address            = address;
+    cmd.AddressDtrMode     = OSPI_ADDRESS_DTR_DISABLE;
+    cmd.AddressMode        = OSPI_ADDRESS_1_LINE;
+    cmd.AddressSize        = octoSpi_addressSizeFromValue(addressSize);
+
+    cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE;
+
+    cmd.DummyCycles        = dummyCycles;
+
+    cmd.DataDtrMode        = OSPI_DATA_DTR_DISABLE;
+    cmd.DataMode           = OSPI_DATA_1_LINE;
+    cmd.NbData             = length;
+
+    cmd.SIOOMode           = OSPI_SIOO_INST_EVERY_CMD;
+    cmd.DQSMode            = OSPI_DQS_DISABLE;
+
+    ErrorStatus status = octoSpiCommand(instance, &cmd);
+
+    if (status == SUCCESS) {
+        status = octoSpiReceive(instance, in);
+    }
+
+    return status == SUCCESS;
+}
+
+MMFLASH_CODE_NOINLINE bool octoSpiReceiveWithAddress4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, uint8_t *in, int length)
+{
+    OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM.
+
+    cmd.OperationType      = OSPI_OPTYPE_COMMON_CFG;
+
+    cmd.Instruction        = instruction;
+    cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE;
+    cmd.InstructionMode    = OSPI_INSTRUCTION_1_LINE;
+    cmd.InstructionSize    = OSPI_INSTRUCTION_8_BITS;
+
+    cmd.Address            = address;
+    cmd.AddressDtrMode     = OSPI_ADDRESS_DTR_DISABLE;
+    cmd.AddressMode        = OSPI_ADDRESS_1_LINE;
+    cmd.AddressSize        = octoSpi_addressSizeFromValue(addressSize);
+
+    cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE;
+
+    cmd.DummyCycles        = dummyCycles;
+
+    cmd.DataDtrMode        = OSPI_DATA_DTR_DISABLE;
+    cmd.DataMode           = OSPI_DATA_4_LINES;
+    cmd.NbData             = length;
+
+    cmd.DQSMode            = OSPI_DQS_DISABLE;
+    cmd.SIOOMode           = OSPI_SIOO_INST_EVERY_CMD;
+
+    ErrorStatus status = octoSpiCommand(instance, &cmd);
+
+    if (status == SUCCESS) {
+        status = octoSpiReceive(instance, in);
+    }
+
+    return status == SUCCESS;
+}
+
+MMFLASH_CODE_NOINLINE bool octoSpiTransmitWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length)
+{
+    OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM.
+
+    cmd.OperationType      = OSPI_OPTYPE_COMMON_CFG;
+
+    cmd.Instruction        = instruction;
+    cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE;
+    cmd.InstructionMode    = OSPI_INSTRUCTION_1_LINE;
+    cmd.InstructionSize    = OSPI_INSTRUCTION_8_BITS;
+
+    cmd.Address            = address;
+    cmd.AddressDtrMode     = OSPI_ADDRESS_DTR_DISABLE;
+    cmd.AddressMode        = OSPI_ADDRESS_1_LINE;
+    cmd.AddressSize        = octoSpi_addressSizeFromValue(addressSize);
+
+    cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE;
+
+    cmd.DummyCycles        = dummyCycles;
+
+    cmd.DataDtrMode        = OSPI_DATA_DTR_DISABLE;
+    cmd.DataMode           = OSPI_DATA_1_LINE;
+    cmd.NbData             = length;
+
+    cmd.DQSMode            = OSPI_DQS_DISABLE;
+    cmd.SIOOMode           = OSPI_SIOO_INST_EVERY_CMD;
+
+    ErrorStatus status = octoSpiCommand(instance, &cmd);
+
+    if (status == SUCCESS) {
+        status = octoSpiTransmit(instance, (uint8_t *)out);
+    }
+
+    return status == SUCCESS;
+}
+
+MMFLASH_CODE_NOINLINE bool octoSpiTransmitWithAddress4LINES(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize, const uint8_t *out, int length)
+{
+    OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM.
+
+    cmd.OperationType      = OSPI_OPTYPE_COMMON_CFG;
+
+    cmd.Instruction        = instruction;
+    cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE;
+    cmd.InstructionMode    = OSPI_INSTRUCTION_1_LINE;
+    cmd.InstructionSize    = OSPI_INSTRUCTION_8_BITS;
+
+    cmd.Address            = address;
+    cmd.AddressDtrMode     = OSPI_ADDRESS_DTR_DISABLE;
+    cmd.AddressMode        = OSPI_ADDRESS_1_LINE;
+    cmd.AddressSize        = octoSpi_addressSizeFromValue(addressSize);
+
+    cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE;
+
+    cmd.DummyCycles        = dummyCycles;
+
+    cmd.DataDtrMode        = OSPI_DATA_DTR_DISABLE;
+    cmd.DataMode           = OSPI_DATA_4_LINES;
+    cmd.NbData             = length;
+
+    cmd.DQSMode            = OSPI_DQS_DISABLE;
+    cmd.SIOOMode           = OSPI_SIOO_INST_EVERY_CMD;
+
+    ErrorStatus status = octoSpiCommand(instance, &cmd);
+
+    if (status == SUCCESS) {
+        status = octoSpiTransmit(instance, (uint8_t *)out);
+    }
+
+
+    return status == SUCCESS;
+}
+
+MMFLASH_CODE_NOINLINE bool octoSpiInstructionWithAddress1LINE(OCTOSPI_TypeDef *instance, uint8_t instruction, uint8_t dummyCycles, uint32_t address, uint8_t addressSize)
+{
+    OSPI_Command_t cmd; // Can't initialise to zero as compiler optimization will use memset() which is not in RAM.
+
+    cmd.OperationType      = OSPI_OPTYPE_COMMON_CFG;
+
+    cmd.Instruction        = instruction;
+    cmd.InstructionDtrMode = OSPI_INSTRUCTION_DTR_DISABLE;
+    cmd.InstructionMode    = OSPI_INSTRUCTION_1_LINE;
+    cmd.InstructionSize    = OSPI_INSTRUCTION_8_BITS;
+
+    cmd.Address            = address;
+    cmd.AddressDtrMode     = OSPI_ADDRESS_DTR_DISABLE;
+    cmd.AddressMode        = OSPI_ADDRESS_1_LINE;
+    cmd.AddressSize        = octoSpi_addressSizeFromValue(addressSize);
+
+    cmd.AlternateBytesMode = OSPI_ALTERNATE_BYTES_NONE;
+
+    cmd.DummyCycles        = dummyCycles;
+
+    cmd.DataDtrMode        = OSPI_DATA_DTR_DISABLE;
+    cmd.DataMode           = OSPI_DATA_NONE;
+    cmd.NbData             = 0;
+
+    cmd.DQSMode            = OSPI_DQS_DISABLE;
+    cmd.SIOOMode           = OSPI_SIOO_INST_EVERY_CMD;
+
+    ErrorStatus status = octoSpiCommand(instance, &cmd);
+
+    return status == SUCCESS;
+}
+
+
+void octoSpiInitDevice(OCTOSPIDevice device)
+{
+    octoSpiDevice_t *octoSpi = &(octoSpiDevice[device]);
+
+#if defined(STM32H730xx) || defined(STM32H723xx)
+    if (isMemoryMappedModeEnabledOnBoot()) {
+        // Bootloader has already configured the IO, clocks and peripherals.
+        octoSpiBackupMemoryMappedModeConfiguration(octoSpi->dev);
+
+        octoSpiTestEnableDisableMemoryMappedMode(octoSpi);
+    } else {
+        failureMode(FAILURE_DEVELOPER); // trying to use this implementation when memory mapped mode is not already enabled by a bootloader
+
+        // Here is where we would configure the OCTOSPI1/2 and OCTOSPIM peripherals for the non-memory-mapped use case.
+    }
+#else
+#error MCU not supported.
+#endif
+
+}
+
+#endif
diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c
index 5de25e0c520..348eebece7a 100644
--- a/src/main/drivers/bus_spi.c
+++ b/src/main/drivers/bus_spi.c
@@ -156,6 +156,10 @@ bool spiInit(SPIDevice device)
 // Return true if DMA engine is busy
 bool spiIsBusy(const extDevice_t *dev)
 {
+    if (dev->bus->csLockDevice && (dev->bus->csLockDevice != dev)) {
+        // If CS is still asserted, but not by the current device, the bus is busy
+        return true;
+    }
     return (dev->bus->curSegment != (busSegment_t *)BUS_SPI_FREE);
 }
 
@@ -163,7 +167,7 @@ bool spiIsBusy(const extDevice_t *dev)
 void spiWait(const extDevice_t *dev)
 {
     // Wait for completion
-    while (dev->bus->curSegment != (busSegment_t *)BUS_SPI_FREE);
+    while (spiIsBusy(dev));
 }
 
 // Wait for bus to become free, then read/write block of data
@@ -419,6 +423,11 @@ FAST_IRQ_HANDLER static void spiIrqHandler(const extDevice_t *dev)
             spiSequenceStart(nextDev);
         } else {
             // The end of the segment list has been reached, so mark transactions as complete
+            if (bus->curSegment->negateCS) {
+                bus->csLockDevice = (extDevice_t *)NULL;
+             } else {
+                bus->csLockDevice = (extDevice_t *)dev;
+            }
             bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
         }
     } else {
@@ -729,6 +738,11 @@ void spiSequence(const extDevice_t *dev, busSegment_t *segments)
             // Safe to discard the volatile qualifier as we're in an atomic block
             busSegment_t *endCmpSegment = (busSegment_t *)bus->curSegment;
 
+            /* It is possible that the endCmpSegment may be NULL as the bus is held busy by csLockDevice.
+             * If this is the case this transfer will be silently dropped. Therefore holding CS low after a transfer,
+             * as is done with the SD card, MUST not be done on a bus where interrupts may trigger a transfer
+             * on an idle bus, such as would be the case with a gyro. This would be result in skipped gyro transfers.
+             */
             if (endCmpSegment) {
                 while (true) {
                     // Find the last segment of the current transfer
@@ -750,11 +764,11 @@ void spiSequence(const extDevice_t *dev, busSegment_t *segments)
                         endCmpSegment = (busSegment_t *)endCmpSegment->u.link.segments;
                     }
                 }
-            }
 
-            // Record the dev and segments parameters in the terminating segment entry
-            endCmpSegment->u.link.dev = dev;
-            endCmpSegment->u.link.segments = segments;
+                // Record the dev and segments parameters in the terminating segment entry
+                endCmpSegment->u.link.dev = dev;
+                endCmpSegment->u.link.segments = segments;
+            }
 
             return;
         } else {
diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c
index 57919e7a7d6..70de9c31733 100644
--- a/src/main/drivers/bus_spi_ll.c
+++ b/src/main/drivers/bus_spi_ll.c
@@ -398,13 +398,11 @@ void spiInternalStartDMA(const extDevice_t *dev)
         LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
         LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx);
 
-        LL_SPI_EnableDMAReq_RX(dev->bus->busType_u.spi.instance);
-
         // Enable channels
         LL_DMA_EnableChannel(dmaTx->dma, dmaTx->stream);
         LL_DMA_EnableChannel(dmaRx->dma, dmaRx->stream);
 
-        LL_SPI_EnableDMAReq_TX(dev->bus->busType_u.spi.instance);
+        SET_BIT(dev->bus->busType_u.spi.instance->CR2, SPI_CR2_TXDMAEN | SPI_CR2_RXDMAEN);
 #else
         DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
         DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index 58b8354838a..d20979fb433 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -35,14 +35,14 @@ typedef enum {
 } displayPortDeviceType_e;
 
 typedef enum {
-    DISPLAYPORT_ATTR_NORMAL = 0,
-    DISPLAYPORT_ATTR_INFO,
-    DISPLAYPORT_ATTR_WARNING,
-    DISPLAYPORT_ATTR_CRITICAL,
-    DISPLAYPORT_ATTR_COUNT,
+    DISPLAYPORT_SEVERITY_NORMAL = 0,
+    DISPLAYPORT_SEVERITY_INFO,
+    DISPLAYPORT_SEVERITY_WARNING,
+    DISPLAYPORT_SEVERITY_CRITICAL,
+    DISPLAYPORT_SEVERITY_COUNT,
 } displayPortSeverity_e;
 
-#define DISPLAYPORT_ATTR_BLINK  0x80 // Device local blink bit or'ed into displayPortSeverity_e
+#define DISPLAYPORT_BLINK  0x80 // Device local blink bit or'ed into displayPortSeverity_e
 
 // System elements rendered by VTX or Goggles
 typedef enum {
diff --git a/src/main/drivers/dshot_bitbang.c b/src/main/drivers/dshot_bitbang.c
index 296ddc42c9b..52eff238259 100644
--- a/src/main/drivers/dshot_bitbang.c
+++ b/src/main/drivers/dshot_bitbang.c
@@ -46,6 +46,16 @@
 #include "drivers/timer.h"
 
 #include "pg/motor.h"
+#include "pg/pinio.h"
+
+// DEBUG_DSHOT_TELEMETRY_COUNTS
+// 0 - Count of telemetry packets read
+// 1 - Count of missing edge
+// 2 - Count of reception not complete in time
+// 3 - Number of high bits before telemetry start
+
+// Maximum time to wait for telemetry reception to complete
+#define DSHOT_TELEMETRY_TIMEOUT 2000
 
 FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS];  // TIM1 or TIM8
 FAST_DATA_ZERO_INIT int usedMotorPacers = 0;
@@ -324,9 +334,12 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
 #ifdef USE_DSHOT_TELEMETRY
     if (useDshotTelemetry) {
         if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
+            bbPort->telemetryPending = false;
 #ifdef DEBUG_COUNT_INTERRUPT
             bbPort->inputIrq++;
 #endif
+            // Disable DMA as telemetry reception is complete
+            bbDMA_Cmd(bbPort, DISABLE);
         } else {
 #ifdef DEBUG_COUNT_INTERRUPT
             bbPort->outputIrq++;
@@ -335,6 +348,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
             // Switch to input
 
             bbSwitchToInput(bbPort);
+            bbPort->telemetryPending = true;
 
             bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, ENABLE);
         }
@@ -441,7 +455,7 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
 
         if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
             bbDevice.vTable.write = motorWriteNull;
-            bbDevice.vTable.updateStart = motorUpdateStartNull;
+            bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
             bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
 
             return false;
@@ -493,49 +507,71 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
     return true;
 }
 
-static bool bbUpdateStart(void)
+static bool bbTelemetryWait(void)
+{
+    // Wait for telemetry reception to complete
+    bool telemetryPending;
+    bool telemetryWait = false;
+    const timeUs_t startTimeUs = micros();
+
+    do {
+        telemetryPending = false;
+        for (int i = 0; i < usedMotorPorts; i++) {
+            telemetryPending |= bbPorts[i].telemetryPending;
+        }
+
+        telemetryWait |= telemetryPending;
+
+        if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
+            break;
+        }
+    } while (telemetryPending);
+
+    if (telemetryWait) {
+        DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
+    }
+
+    return telemetryWait;
+}
+
+static void bbUpdateInit(void)
+{
+    for (int i = 0; i < usedMotorPorts; i++) {
+        bbOutputDataClear(bbPorts[i].portOutputBuffer);
+    }
+}
+
+static bool bbDecodeTelemetry(void)
 {
 #ifdef USE_DSHOT_TELEMETRY
     if (useDshotTelemetry) {
 #ifdef USE_DSHOT_TELEMETRY_STATS
         const timeMs_t currentTimeMs = millis();
 #endif
-        timeUs_t currentUs = micros();
-
-        // don't send while telemetry frames might still be incoming
-        if (cmpTimeUs(currentUs, lastSendUs) < (timeDelta_t)(40 + 2 * dshotFrameUs)) {
-            return false;
-        }
 
-        for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 #ifdef USE_DSHOT_CACHE_MGMT
-            // Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
-            bool invalidated = false;
-            for (int i = 0; i < motorIndex; i++) {
-                if (bbMotors[motorIndex].bbPort->portInputBuffer == bbMotors[i].bbPort->portInputBuffer) {
-                    invalidated = true;
-                }
-            }
-            if (!invalidated) {
-                SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
-                                             DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
-            }
+        for (int i = 0; i < usedMotorPorts; i++) {
+            bbPort_t *bbPort = &bbPorts[i];
+            SCB_InvalidateDCache_by_Addr((uint32_t *)bbPort->portInputBuffer, DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
+        }
 #endif
-
+        for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 #ifdef STM32F4
             uint32_t rawValue = decode_bb_bitband(
                 bbMotors[motorIndex].bbPort->portInputBuffer,
-                bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
+                bbMotors[motorIndex].bbPort->portInputCount,
                 bbMotors[motorIndex].pinIndex);
 #else
             uint32_t rawValue = decode_bb(
                 bbMotors[motorIndex].bbPort->portInputBuffer,
-                bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
+                bbMotors[motorIndex].bbPort->portInputCount,
                 bbMotors[motorIndex].pinIndex);
 #endif
             if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
+                DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
                 continue;
             }
+            DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
             dshotTelemetryState.readCount++;
 
             if (rawValue != DSHOT_TELEMETRY_INVALID) {
@@ -556,10 +592,6 @@ static bool bbUpdateStart(void)
         dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
     }
 #endif
-    for (int i = 0; i < usedMotorPorts; i++) {
-        bbDMA_Cmd(&bbPorts[i], DISABLE);
-        bbOutputDataClear(bbPorts[i].portOutputBuffer);
-    }
 
     return true;
 }
@@ -616,23 +648,11 @@ static void bbUpdateComplete(void)
         }
     }
 
-#ifdef USE_DSHOT_CACHE_MGMT
-    for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
-        // Only clean each buffer once. If all motors are on a common port they'll share a buffer.
-        bool clean = false;
-        for (int i = 0; i < motorIndex; i++) {
-            if (bbMotors[motorIndex].bbPort->portOutputBuffer == bbMotors[i].bbPort->portOutputBuffer) {
-                clean = true;
-            }
-        }
-        if (!clean) {
-            SCB_CleanDCache_by_Addr(bbMotors[motorIndex].bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES);
-        }
-    }
-#endif
-
     for (int i = 0; i < usedMotorPorts; i++) {
         bbPort_t *bbPort = &bbPorts[i];
+#ifdef USE_DSHOT_CACHE_MGMT
+        SCB_CleanDCache_by_Addr(bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES);
+#endif
 
 #ifdef USE_DSHOT_TELEMETRY
         if (useDshotTelemetry) {
@@ -708,7 +728,9 @@ static motorVTable_t bbVTable = {
     .enable = bbEnableMotors,
     .disable = bbDisableMotors,
     .isMotorEnabled = bbIsMotorEnabled,
-    .updateStart = bbUpdateStart,
+    .telemetryWait = bbTelemetryWait,
+    .decodeTelemetry = bbDecodeTelemetry,
+    .updateInit = bbUpdateInit,
     .write = bbWrite,
     .writeInt = bbWriteInt,
     .updateComplete = bbUpdateComplete,
@@ -755,7 +777,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
         if (!IOIsFreeOrPreinit(io)) {
             /* not enough motors initialised for the mixer or a break in the motors */
             bbDevice.vTable.write = motorWriteNull;
-            bbDevice.vTable.updateStart = motorUpdateStartNull;
+            bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
             bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
             bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
             return NULL;
diff --git a/src/main/drivers/dshot_bitbang_decode.c b/src/main/drivers/dshot_bitbang_decode.c
index 388b4dc32cf..54e6aa178fe 100644
--- a/src/main/drivers/dshot_bitbang_decode.c
+++ b/src/main/drivers/dshot_bitbang_decode.c
@@ -1,3 +1,23 @@
+/*
+ * 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/>.
+ */
+
 #include <stdint.h>
 #include <string.h>
 #include <stdio.h>
@@ -8,8 +28,10 @@
 
 #include "common/maths.h"
 #include "common/utils.h"
+#include "build/debug.h"
 #include "drivers/dshot.h"
 #include "drivers/dshot_bitbang_decode.h"
+#include "drivers/time.h"
 
 #define MIN_VALID_BBSAMPLES ((21 - 2) * 3)
 #define MAX_VALID_BBSAMPLES ((21 + 2) * 3)
@@ -28,7 +50,16 @@ uint16_t bbBuffer[134];
 #define BITBAND_SRAM_BASE  0x22000000
 #define BITBAND_SRAM(a,b) ((BITBAND_SRAM_BASE + (((a)-BITBAND_SRAM_REF)<<5) + ((b)<<2)))  // Convert SRAM address
 
+// Period at which to check preamble length
+#define MARGIN_CHECK_INTERVAL_US 500000
+
+// Target 5 clock cycles of input data ahead of leading edge
+#define DSHOT_TELEMETRY_START_MARGIN 5
+
+static uint32_t minMargin = UINT32_MAX;
+static timeUs_t nextMarginCheckUs = 0;
 
+static uint8_t preambleSkip = 0;
 
 typedef struct bitBandWord_s {
     uint32_t value;
@@ -81,9 +112,10 @@ static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t coun
     return value;
 }
 
-
+#ifdef USE_DSHOT_BITBAND
 uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
 {
+    timeUs_t now = micros();
 #ifdef DEBUG_BBDECODE
     memset(sequence, 0, sizeof(sequence));
     sequenceIndex = 0;
@@ -94,6 +126,11 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
     bitBandWord_t* b = p;
     bitBandWord_t* endP = p + (count - MIN_VALID_BBSAMPLES);
 
+    // Jump forward in the buffer to just before where we anticipate the first zero
+    p += preambleSkip;
+
+    DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, preambleSkip);
+
     // Eliminate leading high signal level by looking for first zero bit in data stream.
     // Manual loop unrolling and branch hinting to produce faster code.
     while (p < endP) {
@@ -105,10 +142,16 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
         }
     }
 
+    const uint32_t startMargin = p - b;
+
     if (p >= endP) {
         // not returning telemetry is ok if the esc cpu is
         // overburdened.  in that case no edge will be found and
         // BB_NOEDGE indicates the condition to caller
+        if (preambleSkip > 0) {
+            // Increase the start margin
+            preambleSkip--;
+        }
         return DSHOT_TELEMETRY_NOEDGE;
     }
 
@@ -174,6 +217,7 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
         }
     }
 
+    // length of last sequence has to be inferred since the last bit with inverted dshot is high
     if (bits < 18) {
         return DSHOT_TELEMETRY_NOEDGE;
     }
@@ -184,10 +228,34 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
         return DSHOT_TELEMETRY_NOEDGE;
     }
 
+    // Data appears valid
+    if (startMargin < minMargin) {
+        minMargin = startMargin;
+    }
+
+    if (cmpTimeUs(now, nextMarginCheckUs) >= 0) {
+        nextMarginCheckUs += MARGIN_CHECK_INTERVAL_US;
+
+        // Handle a skipped check
+        if (nextMarginCheckUs < now) {
+            nextMarginCheckUs = now + DSHOT_TELEMETRY_START_MARGIN;
+        }
+
+        if (minMargin > DSHOT_TELEMETRY_START_MARGIN) {
+            preambleSkip = minMargin - DSHOT_TELEMETRY_START_MARGIN;
+        } else {
+            preambleSkip = 0;
+        }
+
+        minMargin = UINT32_MAX;
+    }
+
 #ifdef DEBUG_BBDECODE
     sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3;
     sequenceIndex++;
 #endif
+
+    // The anticipated edges were observed
     if (nlen > 0) {
         value <<= nlen;
         value |= 1 << (nlen - 1);
@@ -196,8 +264,11 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
     return decode_bb_value(value, buffer, count, bit);
 }
 
+#else // USE_DSHOT_BITBAND
+
 FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
 {
+    timeUs_t now = micros();
 #ifdef DEBUG_BBDECODE
     memset(sequence, 0, sizeof(sequence));
     sequenceIndex = 0;
@@ -209,12 +280,17 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
     memset(sequence, 0, sizeof(sequence));
     int sequenceIndex = 0;
 #endif
-
     uint16_t lastValue = 0;
     uint32_t value = 0;
 
     uint16_t* p = buffer;
     uint16_t* endP = p + count - MIN_VALID_BBSAMPLES;
+
+    // Jump forward in the buffer to just before where we anticipate the first zero
+    p += preambleSkip;
+
+    DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, preambleSkip);
+
     // Eliminate leading high signal level by looking for first zero bit in data stream.
     // Manual loop unrolling and branch hinting to produce faster code.
     while (p < endP) {
@@ -225,11 +301,17 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
             break;
         }
     }
+    const uint32_t startMargin = p - buffer;
 
-    if(*p & mask) {
+    if (p >= endP) {
         // not returning telemetry is ok if the esc cpu is
         // overburdened.  in that case no edge will be found and
         // BB_NOEDGE indicates the condition to caller
+        // Increase the start margin
+        if (preambleSkip > 0) {
+            // Increase the start margin
+            preambleSkip--;
+        }
         return DSHOT_TELEMETRY_NOEDGE;
     }
 
@@ -271,16 +353,40 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
         return DSHOT_TELEMETRY_NOEDGE;
     }
 
+    // length of last sequence has to be inferred since the last bit with inverted dshot is high
     const int nlen = 21 - bits;
+    if (nlen < 0) {
+        return DSHOT_TELEMETRY_NOEDGE;
+    }
+
+    // Data appears valid
+    if (startMargin < minMargin) {
+        minMargin = startMargin;
+    }
+
+    if (cmpTimeUs(now, nextMarginCheckUs) >= 0) {
+        nextMarginCheckUs += MARGIN_CHECK_INTERVAL_US;
+
+        // Handle a skipped check
+        if (nextMarginCheckUs < now) {
+            nextMarginCheckUs = now + DSHOT_TELEMETRY_START_MARGIN;
+        }
+
+        if (minMargin > DSHOT_TELEMETRY_START_MARGIN) {
+            preambleSkip = minMargin - DSHOT_TELEMETRY_START_MARGIN;
+        } else {
+            preambleSkip = 0;
+        }
+
+        minMargin = UINT32_MAX;
+    }
+
 #ifdef DEBUG_BBDECODE
     sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3;
     sequenceIndex++;
 #endif
 
-    if (nlen < 0) {
-        return DSHOT_TELEMETRY_NOEDGE;
-    }
-
+    // The anticipated edges were observed
     if (nlen > 0) {
         value <<= nlen;
         value |= 1 << (nlen - 1);
@@ -288,5 +394,6 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
 
     return decode_bb_value(value, buffer, count, bit);
 }
+#endif // USE_DSHOT_BITBAND
 
 #endif
diff --git a/src/main/drivers/dshot_bitbang_decode.h b/src/main/drivers/dshot_bitbang_decode.h
index cddb5764b80..cb7502e8592 100644
--- a/src/main/drivers/dshot_bitbang_decode.h
+++ b/src/main/drivers/dshot_bitbang_decode.h
@@ -18,14 +18,14 @@
  * If not, see <http://www.gnu.org/licenses/>.
  */
 
-
+#include "platform.h"
 
 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
 
-
-
-uint32_t decode_bb(uint16_t buffer[], uint32_t count, uint32_t mask);
+#ifdef USE_DSHOT_BITBAND
 uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit);
-
+#else
+uint32_t decode_bb(uint16_t buffer[], uint32_t count, uint32_t mask);
+#endif
 
 #endif
diff --git a/src/main/drivers/dshot_bitbang_impl.h b/src/main/drivers/dshot_bitbang_impl.h
index f81fcc46db4..928fbf4fc19 100644
--- a/src/main/drivers/dshot_bitbang_impl.h
+++ b/src/main/drivers/dshot_bitbang_impl.h
@@ -167,6 +167,7 @@ typedef struct bbPort_s {
     uint16_t *portInputBuffer;
     uint32_t portInputCount;
     bool inputActive;
+    volatile bool telemetryPending;
 
     // Misc
 #ifdef DEBUG_COUNT_INTERRUPT
diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c
index 37d02b7f219..3421ce019a6 100644
--- a/src/main/drivers/dshot_command.c
+++ b/src/main/drivers/dshot_command.c
@@ -182,6 +182,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
 
     uint8_t repeats = 1;
     timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
+    motorVTable_t *vTable = motorGetVTable();
 
     switch (command) {
     case DSHOT_CMD_SPIN_DIRECTION_1:
@@ -216,18 +217,29 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
         for (; repeats; repeats--) {
             delayMicroseconds(DSHOT_COMMAND_DELAY_US);
 
-#ifdef USE_DSHOT_TELEMETRY
-            timeUs_t timeoutUs = micros() + 1000;
-            while (!motorGetVTable().updateStart() &&
-                   cmpTimeUs(timeoutUs, micros()) > 0);
-#endif
+            // Initialise the output buffers
+            if (vTable->updateInit) {
+                vTable->updateInit();
+            }
+
             for (uint8_t i = 0; i < motorDeviceCount(); i++) {
                 motorDmaOutput_t *const motor = getMotorDmaOutput(i);
                 motor->protocolControl.requestTelemetry = true;
-                motorGetVTable().writeInt(i, (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP);
+                vTable->writeInt(i, (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP);
             }
 
-            motorGetVTable().updateComplete();
+            // Don't attempt to write commands to the motors if telemetry is still being received
+            if (vTable->telemetryWait) {
+                (void)vTable->telemetryWait();
+            }
+
+            vTable->updateComplete();
+
+            // Perform the decode of the last data received
+            // New data will be received once the send of motor data, triggered above, completes
+#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
+            vTable->decodeTelemetry();
+#endif
         }
         delayMicroseconds(delayAfterCommandUs);
 
@@ -239,7 +251,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
         dshotCommandControl_t *commandControl = addCommand();
         if (commandControl) {
             commandControl->repeats = repeats;
-            commandControl->delayAfterCommandUs = delayAfterCommandUs;
+            commandControl->delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
             for (unsigned i = 0; i < motorCount; i++) {
                 if (index == i || index == ALL_MOTORS) {
                     commandControl->command[i] = command;
diff --git a/src/main/drivers/dshot_dpwm.c b/src/main/drivers/dshot_dpwm.c
index de26d589da4..a7ab462f510 100644
--- a/src/main/drivers/dshot_dpwm.c
+++ b/src/main/drivers/dshot_dpwm.c
@@ -140,7 +140,7 @@ static motorVTable_t dshotPwmVTable = {
     .enable = dshotPwmEnableMotors,
     .disable = dshotPwmDisableMotors,
     .isMotorEnabled = dshotPwmIsMotorEnabled,
-    .updateStart = motorUpdateStartNull, // May be updated after copying
+    .decodeTelemetry = motorDecodeTelemetryNull, // May be updated after copying
     .write = dshotWrite,
     .writeInt = dshotWriteInt,
     .updateComplete = pwmCompleteDshotMotorUpdate,
@@ -160,7 +160,7 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
 
 #ifdef USE_DSHOT_TELEMETRY
     useDshotTelemetry = motorConfig->useDshotTelemetry;
-    dshotPwmDevice.vTable.updateStart = pwmStartDshotMotorUpdate;
+    dshotPwmDevice.vTable.decodeTelemetry = pwmTelemetryDecode;
 #endif
 
     switch (motorConfig->motorPwmProtocol) {
diff --git a/src/main/drivers/dshot_dpwm.h b/src/main/drivers/dshot_dpwm.h
index dc1b1900721..f2d963a40ae 100644
--- a/src/main/drivers/dshot_dpwm.h
+++ b/src/main/drivers/dshot_dpwm.h
@@ -159,7 +159,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
 void pwmWriteDshotInt(uint8_t index, uint16_t value);
 bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
 #ifdef USE_DSHOT_TELEMETRY
-bool pwmStartDshotMotorUpdate(void);
+bool pwmTelemetryDecode(void);
 #endif
 void pwmCompleteDshotMotorUpdate(void);
 
diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c
index ee8b55a3d77..39be0e9d838 100644
--- a/src/main/drivers/flash.c
+++ b/src/main/drivers/flash.c
@@ -36,8 +36,12 @@
 #include "flash_w25m.h"
 #include "drivers/bus_spi.h"
 #include "drivers/bus_quadspi.h"
+#include "drivers/bus_octospi.h"
 #include "drivers/io.h"
 #include "drivers/time.h"
+#include "drivers/system.h"
+
+#ifdef USE_FLASH_SPI
 
 // 20 MHz max SPI frequency
 #define FLASH_MAX_SPI_CLK_HZ 20000000
@@ -47,13 +51,143 @@
 static extDevice_t devInstance;
 static extDevice_t *dev;
 
+#endif
+
 static flashDevice_t flashDevice;
 static flashPartitionTable_t flashPartitionTable;
 static int flashPartitions = 0;
 
 #define FLASH_INSTRUCTION_RDID 0x9F
 
-#ifdef USE_QUADSPI
+#ifdef USE_FLASH_MEMORY_MAPPED
+MMFLASH_CODE_NOINLINE void flashMemoryMappedModeDisable(void)
+{
+    __disable_irq();
+#ifdef USE_FLASH_OCTOSPI
+    octoSpiDisableMemoryMappedMode(flashDevice.io.handle.octoSpi);
+#else
+#error Invalid configuration - Not implemented
+#endif
+}
+
+MMFLASH_CODE_NOINLINE void flashMemoryMappedModeEnable(void)
+{
+#ifdef USE_FLASH_OCTOSPI
+    octoSpiEnableMemoryMappedMode(flashDevice.io.handle.octoSpi);
+    __enable_irq();
+#else
+#error Invalid configuration - Not implemented
+#endif
+}
+#endif
+
+#ifdef USE_FLASH_OCTOSPI
+MMFLASH_CODE_NOINLINE static bool flashOctoSpiInit(const flashConfig_t *flashConfig)
+{
+    bool detected = false;
+
+    enum {
+        TRY_1LINE = 0, TRY_4LINE, BAIL
+    } phase = TRY_1LINE;
+
+#ifdef USE_FLASH_MEMORY_MAPPED
+    bool memoryMappedModeEnabledOnBoot = isMemoryMappedModeEnabledOnBoot();
+#else
+    bool memoryMappedModeEnabledOnBoot = false;
+#endif
+
+#ifndef USE_OCTOSPI_EXPERIMENTAL
+    if (!memoryMappedModeEnabledOnBoot) {
+        return false; // Not supported yet, enable USE_OCTOSPI_EXPERIMENTAL and test/update implementation as required.
+    }
+#endif
+
+    OCTOSPI_TypeDef *instance = octoSpiInstanceByDevice(OCTOSPI_CFG_TO_DEV(flashConfig->octoSpiDevice));
+
+    flashDevice.io.handle.octoSpi = instance;
+    flashDevice.io.mode = FLASHIO_OCTOSPI;
+
+    if (memoryMappedModeEnabledOnBoot) {
+        flashMemoryMappedModeDisable();
+    }
+
+    do {
+#ifdef USE_OCTOSPI_EXPERIMENTAL
+        if (!memoryMappedMode) {
+            octoSpiSetDivisor(instance, OCTOSPI_CLOCK_INITIALISATION);
+        }
+#endif
+        // for the memory-mapped use-case, we rely on the bootloader to have already selected the correct speed for the flash chip.
+
+        // 3 bytes for what we need, but some IC's need 8 dummy cycles after the instruction, so read 4 and make two attempts to
+        // assemble the chip id from the response.
+        uint8_t readIdResponse[4];
+
+        bool status = false;
+        switch (phase) {
+        case TRY_1LINE:
+            status = octoSpiReceive1LINE(instance, FLASH_INSTRUCTION_RDID, 0, readIdResponse, 4);
+            break;
+        case TRY_4LINE:
+            status = octoSpiReceive4LINES(instance, FLASH_INSTRUCTION_RDID, 2, readIdResponse, 3);
+            break;
+        default:
+            break;
+        }
+
+        if (!status) {
+            phase++;
+            continue;
+        }
+
+#ifdef USE_OCTOSPI_EXPERIMENTAL
+        if (!memoryMappedModeEnabledOnBoot) {
+            octoSpiSetDivisor(instance, OCTOSPI_CLOCK_ULTRAFAST);
+        }
+#endif
+
+        for (uint8_t offset = 0; offset <= 1 && !detected; offset++) {
+
+            uint32_t jedecID = (readIdResponse[offset + 0] << 16) | (readIdResponse[offset + 1] << 8) | (readIdResponse[offset + 2]);
+
+            if (offset == 0) {
+#if defined(USE_FLASH_W25Q128FV)
+                if (!detected && w25q128fv_identify(&flashDevice, jedecID)) {
+                    detected = true;
+                }
+#endif
+            }
+
+            if (offset == 1) {
+#ifdef USE_OCTOSPI_EXPERIMENTAL
+                if (!memoryMappedModeEnabledOnBoot) {
+                    // These flash chips DO NOT support memory mapped mode; suitable flash read commands must be available.
+#if defined(USE_FLASH_W25N01G)
+                    if (!detected && w25n01g_identify(&flashDevice, jedecID)) {
+                        detected = true;
+                    }
+#endif
+#if defined(USE_FLASH_W25M02G)
+                    if (!detected && w25m_identify(&flashDevice, jedecID)) {
+                        detected = true;
+                    }
+#endif
+                }
+#endif
+            }
+        }
+        phase++;
+    } while (phase != BAIL && !detected);
+
+    if (memoryMappedModeEnabledOnBoot) {
+        flashMemoryMappedModeEnable();
+    }
+    return detected;
+
+}
+#endif // USE_FLASH_OCTOSPI
+
+#ifdef USE_FLASH_QUADSPI
 static bool flashQuadSpiInit(const flashConfig_t *flashConfig)
 {
     bool detected = false;
@@ -63,6 +197,9 @@ static bool flashQuadSpiInit(const flashConfig_t *flashConfig)
 
     QUADSPI_TypeDef *hqspi = quadSpiInstanceByDevice(QUADSPI_CFG_TO_DEV(flashConfig->quadSpiDevice));
 
+    flashDevice.io.handle.quadSpi = hqspi;
+    flashDevice.io.mode = FLASHIO_QUADSPI;
+
     do {
         quadSpiSetDivisor(hqspi, QUADSPI_CLOCK_INITIALISATION);
 
@@ -87,19 +224,16 @@ static bool flashQuadSpiInit(const flashConfig_t *flashConfig)
             continue;
         }
 
-        flashDevice.io.handle.quadSpi = hqspi;
-        flashDevice.io.mode = FLASHIO_QUADSPI;
-
         quadSpiSetDivisor(hqspi, QUADSPI_CLOCK_ULTRAFAST);
 
 
         for (uint8_t offset = 0; offset <= 1 && !detected; offset++) {
 
-            uint32_t chipID = (readIdResponse[offset + 0] << 16) | (readIdResponse[offset + 1] << 8) | (readIdResponse[offset + 2]);
+            uint32_t jedecID = (readIdResponse[offset + 0] << 16) | (readIdResponse[offset + 1] << 8) | (readIdResponse[offset + 2]);
 
             if (offset == 0) {
-#ifdef USE_FLASH_W25Q128FV
-                if (!detected && w25q128fv_detect(&flashDevice, chipID)) {
+#if defined(USE_FLASH_W25Q128FV)
+                if (!detected && w25q128fv_identify(&flashDevice, jedecID)) {
                     detected = true;
                 }
 #endif
@@ -112,20 +246,20 @@ static bool flashQuadSpiInit(const flashConfig_t *flashConfig)
             }
 
             if (offset == 1) {
-#ifdef USE_FLASH_W25N01G
-                if (!detected && w25n01g_detect(&flashDevice, chipID)) {
+#if defined(USE_FLASH_W25N01G)
+                if (!detected && w25n01g_identify(&flashDevice, jedecID)) {
                     detected = true;
                 }
 #endif
 #if defined(USE_FLASH_W25M02G)
-                if (!detected && w25m_detect(&flashDevice, chipID)) {
+                if (!detected && w25m_identify(&flashDevice, jedecID)) {
                     detected = true;
                 }
 #endif
             }
 
             if (detected) {
-                flashDevice.geometry.jedecId = chipID;
+                flashDevice.geometry.jedecId = jedecID;
             }
         }
         phase++;
@@ -133,15 +267,9 @@ static bool flashQuadSpiInit(const flashConfig_t *flashConfig)
 
     return detected;
 }
-#endif  // USE_QUADSPI
-
-#ifdef USE_SPI
-
-void flashPreInit(const flashConfig_t *flashConfig)
-{
-    spiPreinitRegister(flashConfig->csTag, IOCFG_IPU, 1);
-}
+#endif  // USE_FLASH_QUADSPI
 
+#ifdef USE_FLASH_SPI
 static bool flashSpiInit(const flashConfig_t *flashConfig)
 {
     bool detected = false;
@@ -186,39 +314,39 @@ static bool flashSpiInit(const flashConfig_t *flashConfig)
     spiReadRegBuf(dev, FLASH_INSTRUCTION_RDID, readIdResponse, sizeof(readIdResponse));
 
     // Manufacturer, memory type, and capacity
-    uint32_t chipID = (readIdResponse[0] << 16) | (readIdResponse[1] << 8) | (readIdResponse[2]);
+    uint32_t jedecID = (readIdResponse[0] << 16) | (readIdResponse[1] << 8) | (readIdResponse[2]);
 
 #ifdef USE_FLASH_M25P16
-    if (m25p16_detect(&flashDevice, chipID)) {
+    if (m25p16_identify(&flashDevice, jedecID)) {
         detected = true;
     }
 #endif
 
 #if defined(USE_FLASH_W25M512) || defined(USE_FLASH_W25M)
-    if (!detected && w25m_detect(&flashDevice, chipID)) {
+    if (!detected && w25m_identify(&flashDevice, jedecID)) {
         detected = true;
     }
 #endif
 
     if (!detected) {
         // Newer chips
-        chipID = (readIdResponse[1] << 16) | (readIdResponse[2] << 8) | (readIdResponse[3]);
+        jedecID = (readIdResponse[1] << 16) | (readIdResponse[2] << 8) | (readIdResponse[3]);
     }
 
 #ifdef USE_FLASH_W25N01G
-    if (!detected && w25n01g_detect(&flashDevice, chipID)) {
+    if (!detected && w25n01g_identify(&flashDevice, jedecID)) {
         detected = true;
     }
 #endif
 
 #ifdef USE_FLASH_W25M02G
-    if (!detected && w25m_detect(&flashDevice, chipID)) {
+    if (!detected && w25m_identify(&flashDevice, jedecID)) {
         detected = true;
     }
 #endif
 
     if (detected) {
-        flashDevice.geometry.jedecId = chipID;
+        flashDevice.geometry.jedecId = jedecID;
         return detected;
     }
 
@@ -226,39 +354,65 @@ static bool flashSpiInit(const flashConfig_t *flashConfig)
 
     return false;
 }
-#endif // USE_SPI
+#endif // USE_FLASH_SPI
+
+void flashPreInit(const flashConfig_t *flashConfig)
+{
+    spiPreinitRegister(flashConfig->csTag, IOCFG_IPU, 1);
+}
 
 bool flashDeviceInit(const flashConfig_t *flashConfig)
 {
-#ifdef USE_SPI
+    bool haveFlash = false;
+
+#ifdef USE_FLASH_SPI
     bool useSpi = (SPI_CFG_TO_DEV(flashConfig->spiDevice) != SPIINVALID);
 
     if (useSpi) {
-        return flashSpiInit(flashConfig);
+        haveFlash = flashSpiInit(flashConfig);
     }
 #endif
 
-#ifdef USE_QUADSPI
+#ifdef USE_FLASH_QUADSPI
     bool useQuadSpi = (QUADSPI_CFG_TO_DEV(flashConfig->quadSpiDevice) != QUADSPIINVALID);
     if (useQuadSpi) {
-        return flashQuadSpiInit(flashConfig);
+        haveFlash = flashQuadSpiInit(flashConfig);
     }
 #endif
 
-    return false;
+#ifdef USE_FLASH_OCTOSPI
+    bool useOctoSpi = (OCTOSPI_CFG_TO_DEV(flashConfig->octoSpiDevice) != OCTOSPIINVALID);
+    if (useOctoSpi) {
+        haveFlash = flashOctoSpiInit(flashConfig);
+    }
+#endif
+
+    if (haveFlash && flashDevice.vTable->configure) {
+        uint32_t configurationFlags = 0;
+
+#ifdef USE_FLASH_MEMORY_MAPPED
+        if (isMemoryMappedModeEnabledOnBoot()) {
+            configurationFlags |= FLASH_CF_SYSTEM_IS_MEMORY_MAPPED;
+        }
+#endif
+
+        flashDevice.vTable->configure(&flashDevice, configurationFlags);
+    }
+
+    return haveFlash;
 }
 
-bool flashIsReady(void)
+MMFLASH_CODE bool flashIsReady(void)
 {
     return flashDevice.vTable->isReady(&flashDevice);
 }
 
-bool flashWaitForReady(void)
+MMFLASH_CODE bool flashWaitForReady(void)
 {
     return flashDevice.vTable->waitForReady(&flashDevice);
 }
 
-void flashEraseSector(uint32_t address)
+MMFLASH_CODE void flashEraseSector(uint32_t address)
 {
     flashDevice.callback = NULL;
     flashDevice.vTable->eraseSector(&flashDevice, address);
@@ -273,12 +427,12 @@ void flashEraseCompletely(void)
 /* The callback, if provided, will receive the totoal number of bytes transfered
  * by each call to flashPageProgramContinue() once the transfer completes.
  */
-void flashPageProgramBegin(uint32_t address, void (*callback)(uint32_t length))
+MMFLASH_CODE void flashPageProgramBegin(uint32_t address, void (*callback)(uint32_t length))
 {
     flashDevice.vTable->pageProgramBegin(&flashDevice, address, callback);
 }
 
-uint32_t flashPageProgramContinue(const uint8_t **buffers, uint32_t *bufferSizes, uint32_t bufferCount)
+MMFLASH_CODE uint32_t flashPageProgramContinue(const uint8_t **buffers, uint32_t *bufferSizes, uint32_t bufferCount)
 {
     uint32_t maxBytesToWrite = flashDevice.geometry.pageSize - (flashDevice.currentWriteAddress % flashDevice.geometry.pageSize);
 
@@ -299,23 +453,23 @@ uint32_t flashPageProgramContinue(const uint8_t **buffers, uint32_t *bufferSizes
     return flashDevice.vTable->pageProgramContinue(&flashDevice, buffers, bufferSizes, bufferCount);
 }
 
-void flashPageProgramFinish(void)
+MMFLASH_CODE void flashPageProgramFinish(void)
 {
     flashDevice.vTable->pageProgramFinish(&flashDevice);
 }
 
-void flashPageProgram(uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length))
+MMFLASH_CODE void flashPageProgram(uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length))
 {
     flashDevice.vTable->pageProgram(&flashDevice, address, data, length, callback);
 }
 
-int flashReadBytes(uint32_t address, uint8_t *buffer, uint32_t length)
+MMFLASH_CODE int flashReadBytes(uint32_t address, uint8_t *buffer, uint32_t length)
 {
     flashDevice.callback = NULL;
     return flashDevice.vTable->readBytes(&flashDevice, address, buffer, length);
 }
 
-void flashFlush(void)
+MMFLASH_CODE void flashFlush(void)
 {
     if (flashDevice.vTable->flush) {
         flashDevice.vTable->flush(&flashDevice);
@@ -381,7 +535,7 @@ static void flashConfigurePartitions(void)
     startSector = 0;
 #endif
 
-#if defined(CONFIG_IN_EXTERNAL_FLASH)
+#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
     const uint32_t configSize = EEPROM_SIZE;
     flashSector_t configSectors = (configSize / flashGeometry->sectorSize);
 
diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h
index 426f8479fd6..da75304bb6e 100644
--- a/src/main/drivers/flash.h
+++ b/src/main/drivers/flash.h
@@ -48,6 +48,16 @@ typedef struct flashGeometry_s {
     uint32_t jedecId;
 } flashGeometry_t;
 
+
+typedef enum {
+    /*
+     * When set it indicates the system was booted in memory mapped mode, flash chip is already configured by
+     * the bootloader and does not need re-configuration.
+     * When un-set the system was booted normally and the flash chip needs configuration before use.
+     */
+    FLASH_CF_SYSTEM_IS_MEMORY_MAPPED  = (1 << 0),
+} flashConfigurationFlags_e;
+
 void flashPreInit(const flashConfig_t *flashConfig);
 bool flashInit(const flashConfig_t *flashConfig);
 
@@ -63,6 +73,10 @@ int flashReadBytes(uint32_t address, uint8_t *buffer, uint32_t length);
 void flashFlush(void);
 const flashGeometry_t *flashGetGeometry(void);
 
+void flashMemoryMappedModeDisable(void);
+void flashMemoryMappedModeEnable(void);
+
+
 //
 // flash partitioning api
 //
diff --git a/src/main/drivers/flash_impl.h b/src/main/drivers/flash_impl.h
index afa0fdd1a6d..6bd88bd8dbb 100644
--- a/src/main/drivers/flash_impl.h
+++ b/src/main/drivers/flash_impl.h
@@ -22,6 +22,30 @@
  * Author: jflyper
  */
 
+/*
+ * Each flash chip should:
+ *
+ * * expose a public `identify` method.
+ *   - return true if the driver supports the passed JEDEC ID and false otherwise.
+ *   - configure the `geometry` member of the flashDevice_t or set all `geometry` members to 0 if driver doesn't support the JEDEC ID.
+ *   - configure the `vTable` member, with an appropriate API.
+ *   - configure remaining flashDevice_t members, as appropriate.
+ * * not reconfigure the bus or flash chip when in memory mapped mode.
+ *   - the firmware is free to do whatever it wants when memory mapped mode is disabled
+ *   - when memory mapped mode is restored, e.g. after saving config to external flash, it should be in
+ *     the same state that firmware found it in before the firmware disabled memory mapped mode.
+ *
+ * When memory mapped mode is disabled the following applies to all flash chip drivers uses in a memory mapped system:
+ *   - do not call any methods or use data from the flash chip.  i.e. memory mapped code/data is INACCESSIBLE.
+ *     i.e. when saving the config, *ALL* the code to erase a block and write data should be in RAM,
+ *     this includes any `delay` methods.
+ *   - not require the use of use any ISRs - interrupts are disabled during flash access when memory mapped mode is disabled.
+ *
+ * When compiling a driver for use in a memory mapped flash system the following applies:
+ *   - the vTable must live in RAM so it can be used when memory mapped mode is disabled.
+ *   - other constant data structures that usually live in flash memory must be stored in RAM.
+ *   - methods used to erase sectors, write data and read data much live in RAM.
+ */
 #pragma once
 
 #include "drivers/bus.h"
@@ -32,20 +56,30 @@ struct flashVTable_s;
 typedef enum {
     FLASHIO_NONE = 0,
     FLASHIO_SPI,
-    FLASHIO_QUADSPI
+    FLASHIO_QUADSPI,
+    FLASHIO_OCTOSPI,
 } flashDeviceIoMode_e;
 
 typedef struct flashDeviceIO_s {
     union {
+    #ifdef USE_FLASH_SPI
         extDevice_t *dev; // Device interface dependent handle (spi/i2c)
-    #ifdef USE_QUADSPI
+    #endif
+    #ifdef USE_FLASH_QUADSPI
         QUADSPI_TypeDef *quadSpi;
     #endif
+    #ifdef USE_FLASH_OCTOSPI
+        OCTOSPI_TypeDef *octoSpi;
+    #endif
     } handle;
     flashDeviceIoMode_e mode;
 } flashDeviceIO_t;
 
 typedef struct flashDevice_s {
+    //
+    // members to be configured by the flash chip implementation
+    //
+
     const struct flashVTable_s *vTable;
     flashGeometry_t geometry;
     uint32_t currentWriteAddress;
@@ -55,21 +89,33 @@ typedef struct flashDevice_s {
     // when it is definitely ready already.
     bool couldBeBusy;
     uint32_t timeoutAt;
+
+    //
+    // members configured by the flash detection system, read-only from the flash chip implementation's perspective.
+    //
+
     flashDeviceIO_t io;
     void (*callback)(uint32_t arg);
     uint32_t callbackArg;
 } flashDevice_t;
 
 typedef struct flashVTable_s {
+    void (*configure)(flashDevice_t *fdevice, uint32_t configurationFlags);
+
     bool (*isReady)(flashDevice_t *fdevice);
     bool (*waitForReady)(flashDevice_t *fdevice);
+
     void (*eraseSector)(flashDevice_t *fdevice, uint32_t address);
     void (*eraseCompletely)(flashDevice_t *fdevice);
+
     void (*pageProgramBegin)(flashDevice_t *fdevice, uint32_t address, void (*callback)(uint32_t length));
     uint32_t (*pageProgramContinue)(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount);
     void (*pageProgramFinish)(flashDevice_t *fdevice);
     void (*pageProgram)(flashDevice_t *fdevice, uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length));
+
     void (*flush)(flashDevice_t *fdevice);
+
     int (*readBytes)(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, uint32_t length);
+
     const flashGeometry_t *(*getGeometry)(flashDevice_t *fdevice);
 } flashVTable_t;
diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c
index dfa10252f1f..c4a24e1bda1 100644
--- a/src/main/drivers/flash_m25p16.c
+++ b/src/main/drivers/flash_m25p16.c
@@ -195,13 +195,13 @@ static bool m25p16_waitForReady(flashDevice_t *fdevice)
  *
  * Returns true if we get valid ident, false if something bad happened like there is no M25P16.
  */
-bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID)
+bool m25p16_identify(flashDevice_t *fdevice, uint32_t jedecID)
 {
     flashGeometry_t *geometry = &fdevice->geometry;
     uint8_t index;
 
     for (index = 0; m25p16FlashConfig[index].jedecID; index++) {
-        if (m25p16FlashConfig[index].jedecID == chipID) {
+        if (m25p16FlashConfig[index].jedecID == jedecID) {
             maxClkSPIHz = m25p16FlashConfig[index].maxClkSPIMHz * 1000000;
             maxReadClkSPIHz = m25p16FlashConfig[index].maxReadClkSPIMHz * 1000000;
             geometry->sectors = m25p16FlashConfig[index].sectors;
@@ -224,11 +224,32 @@ bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID)
     geometry->sectorSize = geometry->pagesPerSector * geometry->pageSize;
     geometry->totalSize = geometry->sectorSize * geometry->sectors;
 
+    fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be
+    fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be
+
+    if (fdevice->io.mode == FLASHIO_SPI) {
+        fdevice->vTable = &m25p16_vTable;
+    }
+#ifdef USE_QUADSPI
+    else if (fdevice->io.mode == FLASHIO_QUADSPI) {
+        fdevice->vTable = &m25p16Qspi_vTable;
+    }
+#endif
+    return true;
+}
+
+void m25p16_configure(flashDevice_t *fdevice, uint32_t configurationFlags)
+{
+    if (configurationFlags & FLASH_CF_SYSTEM_IS_MEMORY_MAPPED) {
+        return;
+    }
+
     if (fdevice->io.mode == FLASHIO_SPI) {
         // Adjust the SPI bus clock frequency
         spiSetClkDivisor(fdevice->io.handle.dev, spiCalculateDivider(maxReadClkSPIHz));
     }
 
+    flashGeometry_t *geometry = &fdevice->geometry;
     if (geometry->totalSize > 16 * 1024 * 1024) {
         fdevice->isLargeFlash = true;
 
@@ -244,20 +265,9 @@ bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID)
         }
 #endif
     }
-
-    fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be
-
-    if (fdevice->io.mode == FLASHIO_SPI) {
-        fdevice->vTable = &m25p16_vTable;
-    }
-#ifdef USE_QUADSPI
-    else if (fdevice->io.mode == FLASHIO_QUADSPI) {
-        fdevice->vTable = &m25p16Qspi_vTable;
-    }
-#endif
-    return true;
 }
 
+
 static void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLongAddress)
 {
     if (useLongAddress) {
@@ -612,6 +622,7 @@ static const flashGeometry_t* m25p16_getGeometry(flashDevice_t *fdevice)
 }
 
 const flashVTable_t m25p16_vTable = {
+    .configure = m25p16_configure,
     .isReady = m25p16_isReady,
     .waitForReady = m25p16_waitForReady,
     .eraseSector = m25p16_eraseSector,
diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h
index fa606a49475..2e0d186d02f 100644
--- a/src/main/drivers/flash_m25p16.h
+++ b/src/main/drivers/flash_m25p16.h
@@ -22,4 +22,4 @@
 
 #include "flash_impl.h"
 
-bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID);
+bool m25p16_identify(flashDevice_t *fdevice, uint32_t jedecID);
diff --git a/src/main/drivers/flash_w25m.c b/src/main/drivers/flash_w25m.c
index 9600e54175f..34d19ea21a3 100644
--- a/src/main/drivers/flash_w25m.c
+++ b/src/main/drivers/flash_w25m.c
@@ -114,10 +114,10 @@ static bool w25m_waitForReady(flashDevice_t *fdevice)
     return true;
 }
 
-bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID)
+bool w25m_identify(flashDevice_t *fdevice, uint32_t jedecID)
 {
 
-    switch (chipID) {
+    switch (jedecID) {
 #ifdef USE_FLASH_W25M512
     case JEDEC_ID_WINBOND_W25M512:
         // W25Q256 x 2
@@ -127,7 +127,7 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID)
             w25m_dieSelect(fdevice->io.handle.dev, die);
             dieDevice[die].io.handle.dev = fdevice->io.handle.dev;
             dieDevice[die].io.mode = fdevice->io.mode;
-            m25p16_detect(&dieDevice[die], JEDEC_ID_WINBOND_W25Q256);
+            m25p16_identify(&dieDevice[die], JEDEC_ID_WINBOND_W25Q256);
         }
 
         fdevice->geometry.flashType = FLASH_TYPE_NOR;
@@ -143,7 +143,7 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID)
             w25m_dieSelect(fdevice->io.handle.dev, die);
             dieDevice[die].io.handle.dev = fdevice->io.handle.dev;
             dieDevice[die].io.mode = fdevice->io.mode;
-            w25n01g_detect(&dieDevice[die], JEDEC_ID_WINBOND_W25N01GV);
+            w25n01g_identify(&dieDevice[die], JEDEC_ID_WINBOND_W25N01GV);
         }
 
         fdevice->geometry.flashType = FLASH_TYPE_NAND;
@@ -170,6 +170,14 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID)
     return true;
 }
 
+void w25m_configure(flashDevice_t *fdevice, uint32_t configurationFlags)
+{
+    for (int dieNumber = 0 ; dieNumber < dieCount ; dieNumber++) {
+        w25m_dieSelect(fdevice->io.handle.dev, dieNumber);
+        dieDevice[dieNumber].vTable->configure(&dieDevice[dieNumber], configurationFlags);
+    }
+}
+
 void w25m_eraseSector(flashDevice_t *fdevice, uint32_t address)
 {
     int dieNumber = address / dieSize;
@@ -255,6 +263,7 @@ const flashGeometry_t* w25m_getGeometry(flashDevice_t *fdevice)
 }
 
 static const flashVTable_t w25m_vTable = {
+    .configure = w25m_configure,
     .isReady = w25m_isReady,
     .waitForReady = w25m_waitForReady,
     .eraseSector = w25m_eraseSector,
diff --git a/src/main/drivers/flash_w25m.h b/src/main/drivers/flash_w25m.h
index f778a07eb61..5d35df4a4bf 100644
--- a/src/main/drivers/flash_w25m.h
+++ b/src/main/drivers/flash_w25m.h
@@ -22,4 +22,4 @@
 
 #include "flash_impl.h"
 
-bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID);
+bool w25m_identify(flashDevice_t *fdevice, uint32_t jedecID);
diff --git a/src/main/drivers/flash_w25n01g.c b/src/main/drivers/flash_w25n01g.c
index b69f8f18451..87c3f604194 100644
--- a/src/main/drivers/flash_w25n01g.c
+++ b/src/main/drivers/flash_w25n01g.c
@@ -124,9 +124,9 @@
 #define W25N01G_TIMEOUT_RESET_MS            500 // tRSTmax = 500ms
 
 // Sizes (in bits)
-#define W28N01G_STATUS_REGISTER_SIZE        8
-#define W28N01G_STATUS_PAGE_ADDRESS_SIZE    16
-#define W28N01G_STATUS_COLUMN_ADDRESS_SIZE  16
+#define W25N01G_STATUS_REGISTER_SIZE        8
+#define W25N01G_STATUS_PAGE_ADDRESS_SIZE    16
+#define W25N01G_STATUS_COLUMN_ADDRESS_SIZE  16
 
 typedef struct bblut_s {
     uint16_t pba;
@@ -188,7 +188,7 @@ static void w25n01g_performCommandWithPageAddress(flashDeviceIO_t *io, uint8_t c
     else if (io->mode == FLASHIO_QUADSPI) {
         QUADSPI_TypeDef *quadSpi = io->handle.quadSpi;
 
-        quadSpiInstructionWithAddress1LINE(quadSpi, command, 0, pageAddress & 0xffff, W28N01G_STATUS_PAGE_ADDRESS_SIZE + 8);
+        quadSpiInstructionWithAddress1LINE(quadSpi, command, 0, pageAddress & 0xffff, W25N01G_STATUS_PAGE_ADDRESS_SIZE + 8);
     }
 #endif
 }
@@ -221,8 +221,8 @@ static uint8_t w25n01g_readRegister(flashDeviceIO_t *io, uint8_t reg)
 
         QUADSPI_TypeDef *quadSpi = io->handle.quadSpi;
 
-        uint8_t in[1];
-        quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_STATUS_REG, 0, reg, W28N01G_STATUS_REGISTER_SIZE, in, sizeof(in));
+        uint8_t in[W25N01G_STATUS_REGISTER_SIZE / 8];
+        quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_STATUS_REG, 0, reg, W25N01G_STATUS_REGISTER_SIZE, in, sizeof(in));
 
         return in[0];
     }
@@ -253,7 +253,7 @@ static void w25n01g_writeRegister(flashDeviceIO_t *io, uint8_t reg, uint8_t data
    else if (io->mode == FLASHIO_QUADSPI) {
        QUADSPI_TypeDef *quadSpi = io->handle.quadSpi;
 
-       quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_WRITE_STATUS_REG, 0, reg, W28N01G_STATUS_REGISTER_SIZE, &data, 1);
+       quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_WRITE_STATUS_REG, 0, reg, W25N01G_STATUS_REGISTER_SIZE, &data, 1);
    }
 #endif
 }
@@ -311,18 +311,11 @@ static void w25n01g_writeEnable(flashDevice_t *fdevice)
     fdevice->couldBeBusy = true;
 }
 
-/**
- * Read chip identification and geometry information (into global `geometry`).
- *
- * Returns true if we get valid ident, false if something bad happened like there is no M25P16.
- */
 const flashVTable_t w25n01g_vTable;
 
-static void w25n01g_deviceInit(flashDevice_t *flashdev);
-
-bool w25n01g_detect(flashDevice_t *fdevice, uint32_t chipID)
+bool w25n01g_identify(flashDevice_t *fdevice, uint32_t jedecID)
 {
-    switch (chipID) {
+    switch (jedecID) {
     case JEDEC_ID_WINBOND_W25N01GV:
         fdevice->geometry.sectors = 1024;      // Blocks
         fdevice->geometry.pagesPerSector = 64; // Pages/Blocks
@@ -348,6 +341,19 @@ bool w25n01g_detect(flashDevice_t *fdevice, uint32_t chipID)
             W25N01G_BB_MANAGEMENT_START_BLOCK + W25N01G_BB_MANAGEMENT_BLOCKS - 1);
 
     fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be
+    fdevice->vTable = &w25n01g_vTable;
+
+    return true;
+}
+
+static void w25n01g_deviceInit(flashDevice_t *flashdev);
+
+
+void w25n01g_configure(flashDevice_t *fdevice, uint32_t configurationFlags)
+{
+    if (configurationFlags & FLASH_CF_SYSTEM_IS_MEMORY_MAPPED) {
+        return;
+    }
 
     w25n01g_deviceReset(fdevice);
 
@@ -366,10 +372,6 @@ bool w25n01g_detect(flashDevice_t *fdevice, uint32_t chipID)
     // If it ever run out, the device becomes unusable.
 
     w25n01g_deviceInit(fdevice);
-
-    fdevice->vTable = &w25n01g_vTable;
-
-    return true;
 }
 
 /**
@@ -422,7 +424,7 @@ static void w25n01g_programDataLoad(flashDevice_t *fdevice, uint16_t columnAddre
    else if (fdevice->io.mode == FLASHIO_QUADSPI) {
        QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi;
 
-       quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, 0, columnAddress, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, data, length);
+       quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, 0, columnAddress, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, data, length);
     }
 #endif
 
@@ -453,7 +455,7 @@ static void w25n01g_randomProgramDataLoad(flashDevice_t *fdevice, uint16_t colum
     else if (fdevice->io.mode == FLASHIO_QUADSPI) {
         QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi;
 
-        quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, 0, columnAddress, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, data, length);
+        quadSpiTransmitWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, 0, columnAddress, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, data, length);
      }
 #endif
 
@@ -696,8 +698,8 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer,
     else if (fdevice->io.mode == FLASHIO_QUADSPI) {
         QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi;
 
-        //quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_DATA, 8, column, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length);
-        quadSpiReceiveWithAddress4LINES(quadSpi, W25N01G_INSTRUCTION_FAST_READ_QUAD_OUTPUT, 8, column, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length);
+        //quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_DATA, 8, column, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length);
+        quadSpiReceiveWithAddress4LINES(quadSpi, W25N01G_INSTRUCTION_FAST_READ_QUAD_OUTPUT, 8, column, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length);
     }
 #endif
 
@@ -765,7 +767,7 @@ int w25n01g_readExtensionBytes(flashDevice_t *fdevice, uint32_t address, uint8_t
     else if (fdevice->io.mode == FLASHIO_QUADSPI) {
         QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi;
 
-        quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_DATA, 8, column, W28N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length);
+        quadSpiReceiveWithAddress1LINE(quadSpi, W25N01G_INSTRUCTION_READ_DATA, 8, column, W25N01G_STATUS_COLUMN_ADDRESS_SIZE, buffer, length);
     }
 #endif
 
@@ -785,6 +787,7 @@ const flashGeometry_t* w25n01g_getGeometry(flashDevice_t *fdevice)
 }
 
 const flashVTable_t w25n01g_vTable = {
+    .configure = w25n01g_configure,
     .isReady = w25n01g_isReady,
     .waitForReady = w25n01g_waitForReady,
     .eraseSector = w25n01g_eraseSector,
diff --git a/src/main/drivers/flash_w25n01g.h b/src/main/drivers/flash_w25n01g.h
index 219a3fa4660..ef80c7b6e32 100644
--- a/src/main/drivers/flash_w25n01g.h
+++ b/src/main/drivers/flash_w25n01g.h
@@ -25,4 +25,4 @@
 // JEDEC ID
 #define JEDEC_ID_WINBOND_W25N01GV    0xEFAA21
 
-bool w25n01g_detect(flashDevice_t *fdevice, uint32_t chipID);
+bool w25n01g_identify(flashDevice_t *fdevice, uint32_t jedecID);
diff --git a/src/main/drivers/flash_w25q128fv.c b/src/main/drivers/flash_w25q128fv.c
index d005f8e8eeb..e8fb03bf134 100644
--- a/src/main/drivers/flash_w25q128fv.c
+++ b/src/main/drivers/flash_w25q128fv.c
@@ -25,7 +25,7 @@
 
 #include "platform.h"
 
-#if defined(USE_FLASH_W25Q128FV) && defined(USE_QUADSPI)
+#if defined(USE_FLASH_W25Q128FV) && (defined(USE_QUADSPI) || defined(USE_OCTOSPI))
 
 #define USE_FLASH_WRITES_USING_4LINES
 #define USE_FLASH_READS_USING_4LINES
@@ -33,15 +33,19 @@
 #include "build/debug.h"
 #include "common/utils.h"
 
+#include "drivers/time.h"
 #include "drivers/flash.h"
 #include "drivers/flash_impl.h"
 #include "drivers/flash_w25q128fv.h"
 #include "drivers/bus_quadspi.h"
+#include "drivers/bus_octospi.h"
 
 // JEDEC ID
-#define JEDEC_ID_WINBOND_W25Q128FV_SPI      0xEF4018
-#define JEDEC_ID_WINBOND_W25Q128FV_QUADSPI  0xEF6018
-#define JEDEC_ID_WINBOND_W25Q128JV_QUADSPI  0xEF7018
+#define JEDEC_ID_WINBOND_W25Q128FV_SPI          0xEF4018
+#define JEDEC_ID_WINBOND_W25Q128FV_QUADSPI      0xEF6018
+#define JEDEC_ID_WINBOND_W25Q128JV_QUADSPI      0xEF7018
+#define JEDEC_ID_WINBOND_W25Q16JV_SPI           0xEF4015
+#define JEDEC_ID_WINBOND_W25Q16JV_DTR_SPI       0xEF7015
 
 // Device size parameters
 #define W25Q128FV_PAGE_SIZE         2048
@@ -90,14 +94,14 @@
 //#define W25Q128FV_INSTRUCTION_WRITE_DISABLE    0x04
 //#define W25Q128FV_INSTRUCTION_PAGE_PROGRAM     0x02
 
-// Timings (2ms minimum to avoid 1 tick advance in consecutive calls to HAL_GetTick).
-#define W25Q128FV_TIMEOUT_PAGE_READ_MS          4
-#define W25Q128FV_TIMEOUT_RESET_MS              2           // tRST = 30us
+// Values from W25Q128FV Datasheet Rev L.
+#define W25Q128FV_TIMEOUT_PAGE_READ_MS          1           // No minimum specified in datasheet
+#define W25Q128FV_TIMEOUT_RESET_MS              1           // tRST = 30us
 #define W25Q128FV_TIMEOUT_BLOCK_ERASE_64KB_MS   2000        // tBE2max = 2000ms, tBE2typ = 150ms
 #define W25Q128FV_TIMEOUT_CHIP_ERASE_MS        (200 * 1000) // tCEmax 200s, tCEtyp = 40s
 
-#define W25Q128FV_TIMEOUT_PAGE_PROGRAM_MS       2           // tPPmax = 700us, tPPtyp = 250us
-#define W25Q128FV_TIMEOUT_WRITE_ENABLE_MS       2
+#define W25Q128FV_TIMEOUT_PAGE_PROGRAM_MS       3           // tPPmax = 3ms, tPPtyp = 0.7ms
+#define W25Q128FV_TIMEOUT_WRITE_ENABLE_MS       1
 
 
 typedef enum {
@@ -115,45 +119,71 @@ w25q128fvState_t w25q128fvState = { 0 };
 static bool w25q128fv_waitForReady(flashDevice_t *fdevice);
 static void w25q128fv_waitForTimeout(flashDevice_t *fdevice);
 
-static void w25q128fv_setTimeout(flashDevice_t *fdevice, uint32_t timeoutMillis)
+MMFLASH_CODE static void w25q128fv_setTimeout(flashDevice_t *fdevice, timeMs_t timeoutMillis)
 {
-    uint32_t now = HAL_GetTick();
-    fdevice->timeoutAt = now + timeoutMillis;
+    timeMs_t nowMs = microsISR() / 1000;
+    fdevice->timeoutAt = nowMs + timeoutMillis;
 }
 
-static void w25q128fv_performOneByteCommand(flashDeviceIO_t *io, uint8_t command)
+MMFLASH_CODE static void w25q128fv_performOneByteCommand(flashDeviceIO_t *io, uint8_t command)
 {
+#if defined(USE_QUADSPI)
     QUADSPI_TypeDef *quadSpi = io->handle.quadSpi;
     quadSpiTransmit1LINE(quadSpi, command, 0, NULL, 0);
+#elif defined(USE_OCTOSPI)
+    OCTOSPI_TypeDef *octoSpi = io->handle.octoSpi;
+    octoSpiTransmit1LINE(octoSpi, command, 0, NULL, 0);
+#endif
+
 }
 
-static void w25q128fv_performCommandWithAddress(flashDeviceIO_t *io, uint8_t command, uint32_t address)
+MMFLASH_CODE static void w25q128fv_performCommandWithAddress(flashDeviceIO_t *io, uint8_t command, uint32_t address)
 {
+#if defined(USE_QUADSPI)
     QUADSPI_TypeDef *quadSpi = io->handle.quadSpi;
 
     quadSpiInstructionWithAddress1LINE(quadSpi, command, 0, address & 0xffffff, W25Q128FV_ADDRESS_BITS);
+#elif defined(USE_OCTOSPI)
+    OCTOSPI_TypeDef *octoSpi = io->handle.octoSpi;
+
+    octoSpiInstructionWithAddress1LINE(octoSpi, command, 0, address & 0xffffff, W25Q128FV_ADDRESS_BITS);
+#endif
 }
 
-static void w25q128fv_writeEnable(flashDevice_t *fdevice)
+MMFLASH_CODE static void w25q128fv_writeEnable(flashDevice_t *fdevice)
 {
     w25q128fv_performOneByteCommand(&fdevice->io, W25Q128FV_INSTRUCTION_WRITE_ENABLE);
 }
 
-static uint8_t w25q128fv_readRegister(flashDeviceIO_t *io, uint8_t command)
+MMFLASH_CODE static uint8_t w25q128fv_readRegister(flashDeviceIO_t *io, uint8_t command)
 {
+    uint8_t in[W25Q128FV_STATUS_REGISTER_BITS / 8] = { 0 };
+#if defined(USE_QUADSPI)
     QUADSPI_TypeDef *quadSpi = io->handle.quadSpi;
 
-    uint8_t in[1];
     quadSpiReceive1LINE(quadSpi, command, 0, in, W25Q128FV_STATUS_REGISTER_BITS / 8);
+#elif defined(USE_OCTOSPI)
+    OCTOSPI_TypeDef *octoSpi = io->handle.octoSpi;
+
+    octoSpiReceive1LINE(octoSpi, command, 0, in, W25Q128FV_STATUS_REGISTER_BITS / 8);
+#endif
+
 
     return in[0];
 }
 
 static void w25q128fv_writeRegister(flashDeviceIO_t *io, uint8_t command, uint8_t data)
 {
+#if defined(USE_QUADSPI)
     QUADSPI_TypeDef *quadSpi = io->handle.quadSpi;
 
-   quadSpiTransmit1LINE(quadSpi, command, 0, &data, W25Q128FV_STATUS_REGISTER_BITS / 8);
+    quadSpiTransmit1LINE(quadSpi, command, 0, &data, W25Q128FV_STATUS_REGISTER_BITS / 8);
+#elif defined(USE_OCTOSPI)
+    OCTOSPI_TypeDef *octoSpi = io->handle.octoSpi;
+
+    octoSpiTransmit1LINE(octoSpi, command, 0, &data, W25Q128FV_STATUS_REGISTER_BITS / 8);
+#endif
+
 }
 
 static void w25q128fv_deviceReset(flashDevice_t *fdevice)
@@ -203,7 +233,7 @@ static void w25q128fv_deviceReset(flashDevice_t *fdevice)
 #endif
 }
 
-bool w25q128fv_isReady(flashDevice_t *fdevice)
+MMFLASH_CODE bool w25q128fv_isReady(flashDevice_t *fdevice)
 {
     uint8_t status = w25q128fv_readRegister(&fdevice->io, W25Q128FV_INSTRUCTION_READ_STATUS1_REG);
 
@@ -212,7 +242,7 @@ bool w25q128fv_isReady(flashDevice_t *fdevice)
     return !busy;
 }
 
-static bool w25q128fv_isWritable(flashDevice_t *fdevice)
+MMFLASH_CODE static bool w25q128fv_isWritable(flashDevice_t *fdevice)
 {
     uint8_t status = w25q128fv_readRegister(&fdevice->io, W25Q128FV_INSTRUCTION_READ_STATUS1_REG);
 
@@ -221,23 +251,23 @@ static bool w25q128fv_isWritable(flashDevice_t *fdevice)
     return writable;
 }
 
-bool w25q128fv_hasTimedOut(flashDevice_t *fdevice)
+MMFLASH_CODE bool w25q128fv_hasTimedOut(flashDevice_t *fdevice)
 {
-    uint32_t now = HAL_GetTick();
-    if (cmp32(now, fdevice->timeoutAt) >= 0) {
+    uint32_t nowMs = microsISR() / 1000;
+    if (cmp32(nowMs, fdevice->timeoutAt) >= 0) {
         return true;
     }
     return false;
 }
 
-void w25q128fv_waitForTimeout(flashDevice_t *fdevice)
+MMFLASH_CODE void w25q128fv_waitForTimeout(flashDevice_t *fdevice)
 {
    while (!w25q128fv_hasTimedOut(fdevice)) { }
 
    fdevice->timeoutAt = 0;
 }
 
-bool w25q128fv_waitForReady(flashDevice_t *fdevice)
+MMFLASH_CODE bool w25q128fv_waitForReady(flashDevice_t *fdevice)
 {
     bool ready = true;
     while (!w25q128fv_isReady(fdevice)) {
@@ -255,15 +285,24 @@ const flashVTable_t w25q128fv_vTable;
 
 static void w25q128fv_deviceInit(flashDevice_t *flashdev);
 
-bool w25q128fv_detect(flashDevice_t *fdevice, uint32_t chipID)
+MMFLASH_CODE_NOINLINE bool w25q128fv_identify(flashDevice_t *fdevice, uint32_t jedecID)
 {
-    switch (chipID) {
+    switch (jedecID) {
     case JEDEC_ID_WINBOND_W25Q128FV_SPI:
     case JEDEC_ID_WINBOND_W25Q128FV_QUADSPI:
     case JEDEC_ID_WINBOND_W25Q128JV_QUADSPI:
         fdevice->geometry.sectors           = 256;
         fdevice->geometry.pagesPerSector    = 256;
         fdevice->geometry.pageSize          = 256;
+        // = 16777216 128MBit 16MB
+        break;
+
+    case JEDEC_ID_WINBOND_W25Q16JV_DTR_SPI:
+    case JEDEC_ID_WINBOND_W25Q16JV_SPI:
+        fdevice->geometry.sectors           = 32;
+        fdevice->geometry.pagesPerSector    = 256;
+        fdevice->geometry.pageSize          = 256;
+        // = 2097152 16MBit 2MB
         break;
 
     default:
@@ -276,7 +315,9 @@ bool w25q128fv_detect(flashDevice_t *fdevice, uint32_t chipID)
     }
 
     // use the chip id to determine the initial interface mode on cold-boot.
-    switch (chipID) {
+    switch (jedecID) {
+    case JEDEC_ID_WINBOND_W25Q16JV_SPI:
+    case JEDEC_ID_WINBOND_W25Q16JV_DTR_SPI:
     case JEDEC_ID_WINBOND_W25Q128FV_SPI:
         w25q128fvState.initialMode = INITIAL_MODE_SPI;
         break;
@@ -294,18 +335,24 @@ bool w25q128fv_detect(flashDevice_t *fdevice, uint32_t chipID)
     fdevice->geometry.sectorSize = fdevice->geometry.pagesPerSector * fdevice->geometry.pageSize;
     fdevice->geometry.totalSize = fdevice->geometry.sectorSize * fdevice->geometry.sectors;
 
-    w25q128fv_deviceReset(fdevice);
-
-    w25q128fv_deviceInit(fdevice);
-
     fdevice->vTable = &w25q128fv_vTable;
 
     return true;
 }
 
-static void w25q128fv_eraseSector(flashDevice_t *fdevice, uint32_t address)
+void w25q128fv_configure(flashDevice_t *fdevice, uint32_t configurationFlags)
 {
+    if (configurationFlags & FLASH_CF_SYSTEM_IS_MEMORY_MAPPED) {
+        return;
+    }
 
+    w25q128fv_deviceReset(fdevice);
+
+    w25q128fv_deviceInit(fdevice);
+}
+
+MMFLASH_CODE static void w25q128fv_eraseSector(flashDevice_t *fdevice, uint32_t address)
+{
     w25q128fv_waitForReady(fdevice);
 
     w25q128fv_writeEnable(fdevice);
@@ -326,17 +373,26 @@ static void w25q128fv_eraseCompletely(flashDevice_t *fdevice)
     w25q128fv_setTimeout(fdevice, W25Q128FV_TIMEOUT_CHIP_ERASE_MS);
 }
 
-
-static void w25q128fv_loadProgramData(flashDevice_t *fdevice, const uint8_t *data, int length)
+MMFLASH_CODE static void w25q128fv_loadProgramData(flashDevice_t *fdevice, const uint8_t *data, int length)
 {
     w25q128fv_waitForReady(fdevice);
 
+#if defined(USE_QUADSPI)
     QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi;
 
 #ifdef USE_FLASH_WRITES_USING_4LINES
     quadSpiTransmitWithAddress4LINES(quadSpi, W25Q128FV_INSTRUCTION_QUAD_PAGE_PROGRAM, 0, w25q128fvState.currentWriteAddress, W25Q128FV_ADDRESS_BITS, data, length);
 #else
     quadSpiTransmitWithAddress1LINE(quadSpi, W25Q128FV_INSTRUCTION_PAGE_PROGRAM, 0, w25q128fvState.currentWriteAddress, W25Q128FV_ADDRESS_BITS, data, length);
+#endif
+#elif defined(USE_OCTOSPI)
+    OCTOSPI_TypeDef *octoSpi = fdevice->io.handle.octoSpi;
+
+#ifdef USE_FLASH_WRITES_USING_4LINES
+    octoSpiTransmitWithAddress4LINES(octoSpi, W25Q128FV_INSTRUCTION_QUAD_PAGE_PROGRAM, 0, w25q128fvState.currentWriteAddress, W25Q128FV_ADDRESS_BITS, data, length);
+#else
+    octoSpiTransmitWithAddress1LINE(octoSpi, W25Q128FV_INSTRUCTION_PAGE_PROGRAM, 0, w25q128fvState.currentWriteAddress, W25Q128FV_ADDRESS_BITS, data, length);
+#endif
 #endif
 
     w25q128fv_setTimeout(fdevice, W25Q128FV_TIMEOUT_PAGE_PROGRAM_MS);
@@ -344,13 +400,13 @@ static void w25q128fv_loadProgramData(flashDevice_t *fdevice, const uint8_t *dat
     w25q128fvState.currentWriteAddress += length;
 }
 
-static void w25q128fv_pageProgramBegin(flashDevice_t *fdevice, uint32_t address, void (*callback)(uint32_t length))
+MMFLASH_CODE static void w25q128fv_pageProgramBegin(flashDevice_t *fdevice, uint32_t address, void (*callback)(uint32_t length))
 {
     fdevice->callback = callback;
     w25q128fvState.currentWriteAddress = address;
 }
 
-static uint32_t w25q128fv_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount)
+MMFLASH_CODE static uint32_t w25q128fv_pageProgramContinue(flashDevice_t *fdevice, uint8_t const **buffers, uint32_t *bufferSizes, uint32_t bufferCount)
 {
     for (uint32_t i = 0; i < bufferCount; i++) {
         w25q128fv_waitForReady(fdevice);
@@ -374,35 +430,45 @@ static uint32_t w25q128fv_pageProgramContinue(flashDevice_t *fdevice, uint8_t co
     return fdevice->callbackArg;
 }
 
-static void w25q128fv_pageProgramFinish(flashDevice_t *fdevice)
+MMFLASH_CODE static void w25q128fv_pageProgramFinish(flashDevice_t *fdevice)
 {
     UNUSED(fdevice);
 }
 
-static void w25q128fv_pageProgram(flashDevice_t *fdevice, uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length))
+MMFLASH_CODE static void w25q128fv_pageProgram(flashDevice_t *fdevice, uint32_t address, const uint8_t *data, uint32_t length, void (*callback)(uint32_t length))
 {
     w25q128fv_pageProgramBegin(fdevice, address, callback);
     w25q128fv_pageProgramContinue(fdevice, &data, &length, 1);
     w25q128fv_pageProgramFinish(fdevice);
 }
 
-void w25q128fv_flush(flashDevice_t *fdevice)
+MMFLASH_CODE void w25q128fv_flush(flashDevice_t *fdevice)
 {
     UNUSED(fdevice);
 }
 
-static int w25q128fv_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, uint32_t length)
+MMFLASH_CODE static int w25q128fv_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, uint32_t length)
 {
     if (!w25q128fv_waitForReady(fdevice)) {
         return 0;
     }
 
+#if defined(USE_QUADSPI)
     QUADSPI_TypeDef *quadSpi = fdevice->io.handle.quadSpi;
 #ifdef USE_FLASH_READS_USING_4LINES
     bool status = quadSpiReceiveWithAddress4LINES(quadSpi, W25Q128FV_INSTRUCTION_FAST_READ_QUAD_OUTPUT, 8, address, W25Q128FV_ADDRESS_BITS, buffer, length);
 #else
     bool status = quadSpiReceiveWithAddress1LINE(quadSpi, W25Q128FV_INSTRUCTION_FAST_READ, 8, address, W25Q128FV_ADDRESS_BITS, buffer, length);
 #endif
+#elif defined(USE_OCTOSPI)
+    OCTOSPI_TypeDef *octoSpi = fdevice->io.handle.octoSpi;
+#ifdef USE_FLASH_READS_USING_4LINES
+    bool status = octoSpiReceiveWithAddress4LINES(octoSpi, W25Q128FV_INSTRUCTION_FAST_READ_QUAD_OUTPUT, 8, address, W25Q128FV_ADDRESS_BITS, buffer, length);
+#else
+    bool status = octoSpiReceiveWithAddress1LINE(octoSpi, W25Q128FV_INSTRUCTION_FAST_READ, 8, address, W25Q128FV_ADDRESS_BITS, buffer, length);
+#endif
+#endif
+
     w25q128fv_setTimeout(fdevice, W25Q128FV_TIMEOUT_PAGE_READ_MS);
 
     if (!status) {
@@ -412,14 +478,13 @@ static int w25q128fv_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t
     return length;
 }
 
-
-
 const flashGeometry_t* w25q128fv_getGeometry(flashDevice_t *fdevice)
 {
     return &fdevice->geometry;
 }
 
-const flashVTable_t w25q128fv_vTable = {
+MMFLASH_DATA const flashVTable_t w25q128fv_vTable = {
+    .configure = w25q128fv_configure,
     .isReady = w25q128fv_isReady,
     .waitForReady = w25q128fv_waitForReady,
     .eraseSector = w25q128fv_eraseSector,
diff --git a/src/main/drivers/flash_w25q128fv.h b/src/main/drivers/flash_w25q128fv.h
index d28de244faf..4a82c0d9984 100644
--- a/src/main/drivers/flash_w25q128fv.h
+++ b/src/main/drivers/flash_w25q128fv.h
@@ -3,5 +3,5 @@
 
 #ifdef USE_FLASH_W25Q128FV
 
-bool w25q128fv_detect(flashDevice_t *fdevice, uint32_t chipID);
+bool w25q128fv_identify(flashDevice_t *fdevice, uint32_t jedecID);
 #endif
diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c
index 77fa985157e..e3d5308d97d 100644
--- a/src/main/drivers/light_ws2811strip.c
+++ b/src/main/drivers/light_ws2811strip.c
@@ -59,8 +59,6 @@ DMA_RW_AXI __attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BU
 #else
 #if defined(STM32F7)
 FAST_DATA_ZERO_INIT uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
-#elif defined(STM32H7)
-DMA_RAM uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
 #else
 uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
 #endif
diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 22a7ec0a9d2..7f461288ac4 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -194,6 +194,7 @@ extDevice_t *dev = &max7456Device;
 
 static bool max7456DeviceDetected = false;
 static uint16_t max7456SpiClockDiv;
+static volatile bool max7456ActiveDma = false;
 
 uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
 
@@ -540,7 +541,7 @@ bool max7456LayerCopy(displayPortLayer_e destLayer, displayPortLayer_e sourceLay
 
 bool max7456DmaInProgress(void)
 {
-    return spiIsBusy(dev);
+    return max7456ActiveDma;
 }
 
 bool max7456BuffersSynced(void)
@@ -611,13 +612,23 @@ bool max7456ReInitIfRequired(bool forceStallCheck)
     return stalled;
 }
 
+// Called in ISR context
+busStatus_e max7456_callbackReady(uint32_t arg)
+{
+    UNUSED(arg);
+
+    max7456ActiveDma = false;
+
+    return BUS_READY;
+}
+
 // Return true if screen still being transferred
 bool max7456DrawScreen(void)
 {
     static uint16_t pos = 0;
     // This routine doesn't block so need to use static data
     static busSegment_t segments[] = {
-            {.u.link = {NULL, NULL}, 0, true, NULL},
+            {.u.link = {NULL, NULL}, 0, true, max7456_callbackReady},
             {.u.link = {NULL, NULL}, 0, true, NULL},
     };
 
@@ -706,6 +717,8 @@ bool max7456DrawScreen(void)
             segments[0].u.buffers.txData = spiBuf;
             segments[0].len = spiBufIndex;
 
+            max7456ActiveDma = true;
+
             spiSequence(dev, &segments[0]);
 
             // Non-blocking, so transfer still in progress if using DMA
diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c
index f6917d0b47c..fc2f11731b3 100644
--- a/src/main/drivers/motor.c
+++ b/src/main/drivers/motor.c
@@ -60,15 +60,49 @@ void motorWriteAll(float *values)
 {
 #ifdef USE_PWM_OUTPUT
     if (motorDevice->enabled) {
+#ifdef USE_DSHOT_BITBANG
+        if (isDshotBitbangActive(&motorConfig()->dev)) {
+            // Initialise the output buffers
+            if (motorDevice->vTable.updateInit) {
+                motorDevice->vTable.updateInit();
+            }
+
+            // Update the motor data
+            for (int i = 0; i < motorDevice->count; i++) {
+                motorDevice->vTable.write(i, values[i]);
+            }
+
+            // Don't attempt to write commands to the motors if telemetry is still being received
+            if (motorDevice->vTable.telemetryWait) {
+                (void)motorDevice->vTable.telemetryWait();
+            }
+
+            // Trigger the transmission of the motor data
+            motorDevice->vTable.updateComplete();
+
+            // Perform the decode of the last data received
+            // New data will be received once the send of motor data, triggered above, completes
 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
-        if (!motorDevice->vTable.updateStart()) {
-            return;
-        }
+            motorDevice->vTable.decodeTelemetry();
+#endif
+        } else
+#endif
+        {
+            // Perform the decode of the last data received
+            // New data will be received once the send of motor data, triggered above, completes
+#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
+            motorDevice->vTable.decodeTelemetry();
 #endif
-        for (int i = 0; i < motorDevice->count; i++) {
-            motorDevice->vTable.write(i, values[i]);
+
+            // Update the motor data
+            for (int i = 0; i < motorDevice->count; i++) {
+                motorDevice->vTable.write(i, values[i]);
+            }
+
+            // Trigger the transmission of the motor data
+            motorDevice->vTable.updateComplete();
+
         }
-        motorDevice->vTable.updateComplete();
     }
 #else
     UNUSED(values);
@@ -80,9 +114,9 @@ unsigned motorDeviceCount(void)
     return motorDevice->count;
 }
 
-motorVTable_t motorGetVTable(void)
+motorVTable_t *motorGetVTable(void)
 {
-    return motorDevice->vTable;
+    return &motorDevice->vTable;
 }
 
 // This is not motor generic anymore; should be moved to analog pwm module
@@ -193,7 +227,7 @@ static bool motorIsEnabledNull(uint8_t index)
     return false;
 }
 
-bool motorUpdateStartNull(void)
+bool motorDecodeTelemetryNull(void)
 {
     return true;
 }
@@ -235,7 +269,7 @@ static const motorVTable_t motorNullVTable = {
     .enable = motorEnableNull,
     .disable = motorDisableNull,
     .isMotorEnabled = motorIsEnabledNull,
-    .updateStart = motorUpdateStartNull,
+    .decodeTelemetry = motorDecodeTelemetryNull,
     .write = motorWriteNull,
     .writeInt = motorWriteIntNull,
     .updateComplete = motorUpdateCompleteNull,
diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h
index b8dcabaf397..9ff075fe750 100644
--- a/src/main/drivers/motor.h
+++ b/src/main/drivers/motor.h
@@ -50,7 +50,9 @@ typedef struct motorVTable_s {
     bool (*enable)(void);
     void (*disable)(void);
     bool (*isMotorEnabled)(uint8_t index);
-    bool (*updateStart)(void);
+    bool (*telemetryWait)(void);
+    bool (*decodeTelemetry)(void);
+    void (*updateInit)(void);
     void (*write)(uint8_t index, float value);
     void (*writeInt)(uint8_t index, uint16_t value);
     void (*updateComplete)(void);
@@ -70,7 +72,7 @@ typedef struct motorDevice_s {
 
 void motorPostInitNull();
 void motorWriteNull(uint8_t index, float value);
-bool motorUpdateStartNull(void);
+bool motorDecodeTelemetryNull(void);
 void motorUpdateCompleteNull(void);
 
 void motorPostInit();
@@ -84,7 +86,7 @@ uint16_t motorConvertToExternal(float motorValue);
 struct motorDevConfig_s; // XXX Shouldn't be needed once pwm_output* is really cleaned up.
 void motorDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount);
 unsigned motorDeviceCount(void);
-motorVTable_t motorGetVTable(void);
+motorVTable_t *motorGetVTable(void);
 bool checkMotorProtocolEnabled(const motorDevConfig_t *motorConfig, bool *protocolIsDshot);
 bool isMotorProtocolDshot(void);
 bool isMotorProtocolEnabled(void);
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 1a031322f02..5c996b7bbd0 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -215,7 +215,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
     }
 
     motorPwmDevice.vTable.write = pwmWriteStandard;
-    motorPwmDevice.vTable.updateStart = motorUpdateStartNull;
+    motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
     motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
 
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c
index 6d92deb338c..c32d3ae366a 100644
--- a/src/main/drivers/pwm_output_dshot_shared.c
+++ b/src/main/drivers/pwm_output_dshot_shared.c
@@ -176,7 +176,11 @@ static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
 #endif
 
 #ifdef USE_DSHOT_TELEMETRY
-FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
+/**
+ * Process dshot telemetry packets before switching the channels back to outputs
+ * 
+*/
+FAST_CODE_NOINLINE bool pwmTelemetryDecode(void)
 {
     if (!useDshotTelemetry) {
         return true;
diff --git a/src/main/drivers/pwm_output_dshot_shared.h b/src/main/drivers/pwm_output_dshot_shared.h
index 9e4ef0faf8a..7c282d50083 100644
--- a/src/main/drivers/pwm_output_dshot_shared.h
+++ b/src/main/drivers/pwm_output_dshot_shared.h
@@ -59,7 +59,7 @@ void pwmDshotSetDirectionOutput(
 #endif
 );
 
-bool pwmStartDshotMotorUpdate(void);
+bool pwmTelemetryDecode(void);
 
 #endif
 #endif
diff --git a/src/main/drivers/rx/rx_sx1280.c b/src/main/drivers/rx/rx_sx1280.c
index 34546b8dffb..b3f7ba988f3 100644
--- a/src/main/drivers/rx/rx_sx1280.c
+++ b/src/main/drivers/rx/rx_sx1280.c
@@ -572,7 +572,7 @@ static void sx1280StartTransmittingDMA(extiCallbackRec_t *cb);
 FAST_IRQ_HANDLER void sx1280ISR(void)
 {
     // Only attempt to access the SX1280 if it is currently idle to avoid any race condition
-    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+    ATOMIC_BLOCK(NVIC_PRIO_RX_INT_EXTI) {
         if (sx1280EnableBusy()) {
             pendingISR = false;
             sx1280SetBusyFn(sx1280IrqGetStatus);
diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h
index 2f63e596aee..f178975e371 100644
--- a/src/main/drivers/serial.h
+++ b/src/main/drivers/serial.h
@@ -53,6 +53,9 @@ typedef enum {
     SERIAL_BIDIR_PP        = 1 << 4,
     SERIAL_BIDIR_NOPULL    = 1 << 5, // disable pulls in BIDIR RX mode
     SERIAL_BIDIR_PP_PD     = 1 << 6, // PP mode, normall inverted, but with PullDowns, to fix SA after bidir issue fixed (#10220)
+
+    // If this option is set then switch the TX line to input when not in use to detect it being pulled low
+    SERIAL_CHECK_TX        = 1 << 7,
 } portOptions_e;
 
 // Define known line control states which may be passed up by underlying serial driver callback
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index b03b80a0ee5..94d753898eb 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -267,6 +267,12 @@ static void uartWrite(serialPort_t *instance, uint8_t ch)
 {
     uartPort_t *uartPort = (uartPort_t *)instance;
 
+    // Check if the TX line is being pulled low by an unpowered peripheral
+    if (uartPort->checkUsartTxOutput && !uartPort->checkUsartTxOutput(uartPort)) {
+        // TX line is being pulled low, so don't transmit
+        return;
+    }
+
     uartPort->port.txBuffer[uartPort->port.txBufferHead] = ch;
 
     if (uartPort->port.txBufferHead + 1 >= uartPort->port.txBufferSize) {
diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h
index b9351355a91..8fc78b0197b 100644
--- a/src/main/drivers/serial_uart.h
+++ b/src/main/drivers/serial_uart.h
@@ -72,6 +72,8 @@ typedef struct uartPort_s {
 #endif
     USART_TypeDef *USARTx;
     bool txDMAEmpty;
+
+    bool (* checkUsartTxOutput)(struct uartPort_s *s);
 } uartPort_t;
 
 void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig);
diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c
index 53e85a5259a..a7074ad5b30 100644
--- a/src/main/drivers/serial_uart_hal.c
+++ b/src/main/drivers/serial_uart_hal.c
@@ -231,11 +231,54 @@ void uartReconfigure(uartPort_t *uartPort)
 
             /* Enable the UART Transmit Data Register Empty Interrupt */
             SET_BIT(uartPort->USARTx->CR1, USART_CR1_TXEIE);
+            SET_BIT(uartPort->USARTx->CR1, USART_CR1_TCIE);
         }
     }
     return;
 }
 
+bool checkUsartTxOutput(uartPort_t *s)
+{
+    uartDevice_t *uart = container_of(s, uartDevice_t, port);
+    IO_t txIO = IOGetByTag(uart->tx.pin);
+
+    if ((uart->txPinState == TX_PIN_MONITOR) && txIO) {
+        if (IORead(txIO)) {
+            // TX is high so we're good to transmit
+
+            // Enable USART TX output
+            uart->txPinState = TX_PIN_ACTIVE;
+            IOConfigGPIOAF(txIO, IOCFG_AF_PP, uart->tx.af);
+
+            // Enable the UART transmitter
+            SET_BIT(s->Handle.Instance->CR1, USART_CR1_TE);
+
+            return true;
+        } else {
+            // TX line is pulled low so don't enable USART TX
+            return false;
+        }
+    }
+
+    return true;
+}
+
+void uartTxMonitor(uartPort_t *s)
+{
+    uartDevice_t *uart = container_of(s, uartDevice_t, port);
+
+    if (uart->txPinState == TX_PIN_ACTIVE) {
+        IO_t txIO = IOGetByTag(uart->tx.pin);
+
+        // Disable the UART transmitter
+        CLEAR_BIT(s->Handle.Instance->CR1, USART_CR1_TE);
+
+        // Switch TX to an input with pullup so it's state can be monitored
+        uart->txPinState = TX_PIN_MONITOR;
+        IOConfigGPIO(txIO, IOCFG_IPU);
+    }
+}
+
 #ifdef USE_DMA
 void uartTryStartTxDMA(uartPort_t *s)
 {
@@ -275,7 +318,14 @@ void uartTryStartTxDMA(uartPort_t *s)
 
 static void handleUsartTxDma(uartPort_t *s)
 {
+    uartDevice_t *uart = container_of(s, uartDevice_t, port);
+
     uartTryStartTxDMA(s);
+
+    if (s->txDMAEmpty && (uart->txPinState != TX_PIN_IGNORE)) {
+        // Switch TX to an input with pullup so it's state can be monitored
+        uartTxMonitor(s);
+    }
 }
 
 void uartDmaIrqHandler(dmaChannelDescriptor_t* descriptor)
@@ -343,7 +393,19 @@ FAST_IRQ_HANDLER void uartIrqHandler(uartPort_t *s)
         __HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF);
     }
 
-    // UART transmitter in interrupting mode, tx buffer empty
+    // UART transmission completed
+    if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) {
+        __HAL_UART_CLEAR_IT(huart, UART_CLEAR_TCF);
+
+        // Switch TX to an input with pull-up so it's state can be monitored
+        uartTxMonitor(s);
+
+#ifdef USE_DMA
+        if (s->txDMAResource) {
+            handleUsartTxDma(s);
+        }
+#endif
+    }
 
     if (
 #ifdef USE_DMA
@@ -351,33 +413,19 @@ FAST_IRQ_HANDLER void uartIrqHandler(uartPort_t *s)
 #endif
         (__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) {
         /* Check that a Tx process is ongoing */
-        if (huart->gState != HAL_UART_STATE_BUSY_TX) {
-            if (s->port.txBufferTail == s->port.txBufferHead) {
-                huart->TxXferCount = 0;
-                /* Disable the UART Transmit Data Register Empty Interrupt */
-                CLEAR_BIT(huart->Instance->CR1, USART_CR1_TXEIE);
+        if (s->port.txBufferTail == s->port.txBufferHead) {
+            /* Disable the UART Transmit Data Register Empty Interrupt */
+            CLEAR_BIT(huart->Instance->CR1, USART_CR1_TXEIE);
+        } else {
+            if ((huart->Init.WordLength == UART_WORDLENGTH_9B) && (huart->Init.Parity == UART_PARITY_NONE)) {
+                huart->Instance->TDR = (((uint16_t) s->port.txBuffer[s->port.txBufferTail]) & (uint16_t) 0x01FFU);
             } else {
-                if ((huart->Init.WordLength == UART_WORDLENGTH_9B) && (huart->Init.Parity == UART_PARITY_NONE)) {
-                    huart->Instance->TDR = (((uint16_t) s->port.txBuffer[s->port.txBufferTail]) & (uint16_t) 0x01FFU);
-                } else {
-                    huart->Instance->TDR = (uint8_t)(s->port.txBuffer[s->port.txBufferTail]);
-                }
-                s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
+                huart->Instance->TDR = (uint8_t)(s->port.txBuffer[s->port.txBufferTail]);
             }
+            s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
         }
     }
 
-    // UART transmitter in DMA mode, transmission completed
-
-    if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) {
-        HAL_UART_IRQHandler(huart);
-#ifdef USE_DMA
-        if (s->txDMAResource) {
-            handleUsartTxDma(s);
-        }
-#endif
-    }
-
     // UART reception idle detected
 
     if (__HAL_UART_GET_IT(huart, UART_IT_IDLE)) {
diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h
index 87e55230c79..bc6e0e427e0 100644
--- a/src/main/drivers/serial_uart_impl.h
+++ b/src/main/drivers/serial_uart_impl.h
@@ -197,6 +197,12 @@ extern const uartHardware_t uartHardware[];
 // uartDevice_t is an actual device instance.
 // XXX Instances are allocated for uarts defined by USE_UARTx atm.
 
+typedef enum {
+    TX_PIN_ACTIVE,
+    TX_PIN_MONITOR,
+    TX_PIN_IGNORE
+} txPinState_t;
+
 typedef struct uartDevice_s {
     uartPort_t port;
     const uartHardware_t *hardware;
@@ -207,6 +213,7 @@ typedef struct uartDevice_s {
 #if !defined(STM32F4) // Don't support pin swap.
     bool pinSwap;
 #endif
+    txPinState_t txPinState;
 } uartDevice_t;
 
 extern uartDevice_t *uartDevmap[];
@@ -225,6 +232,9 @@ void uartConfigureDma(uartDevice_t *uartdev);
 
 void uartDmaIrqHandler(dmaChannelDescriptor_t* descriptor);
 
+bool checkUsartTxOutput(uartPort_t *s);
+void uartTxMonitor(uartPort_t *s);
+
 #if defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
 #define UART_REG_RXD(base) ((base)->RDR)
 #define UART_REG_TXD(base) ((base)->TDR)
diff --git a/src/main/drivers/serial_uart_stdperiph.c b/src/main/drivers/serial_uart_stdperiph.c
index ab6a9142aa3..2d3255e0f39 100644
--- a/src/main/drivers/serial_uart_stdperiph.c
+++ b/src/main/drivers/serial_uart_stdperiph.c
@@ -180,6 +180,8 @@ void uartReconfigure(uartPort_t *uartPort)
         } else {
             USART_ITConfig(uartPort->USARTx, USART_IT_TXE, ENABLE);
         }
+        // Enable the interrupt so completion of the transmission will be signalled
+        USART_ITConfig(uartPort->USARTx, USART_IT_TC, ENABLE);
     }
 
     USART_Cmd(uartPort->USARTx, ENABLE);
diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c
index 0f1dc8dbb1a..57f6b607d7b 100644
--- a/src/main/drivers/serial_uart_stm32f4xx.c
+++ b/src/main/drivers/serial_uart_stm32f4xx.c
@@ -26,6 +26,7 @@
 #include <stdint.h>
 
 #include "platform.h"
+#include "build/debug.h"
 
 #ifdef USE_UART
 
@@ -217,9 +218,58 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #endif
 };
 
+bool checkUsartTxOutput(uartPort_t *s)
+{
+    uartDevice_t *uart = container_of(s, uartDevice_t, port);
+    IO_t txIO = IOGetByTag(uart->tx.pin);
+
+    if ((uart->txPinState == TX_PIN_MONITOR) && txIO) {
+        if (IORead(txIO)) {
+            // TX is high so we're good to transmit
+
+            // Enable USART TX output
+            uart->txPinState = TX_PIN_ACTIVE;
+            IOConfigGPIOAF(txIO, IOCFG_AF_PP, uart->hardware->af);
+
+            // Enable the UART transmitter
+            SET_BIT(s->USARTx->CR1, USART_CR1_TE);
+
+            return true;
+        } else {
+            // TX line is pulled low so don't enable USART TX
+            return false;
+        }
+    }
+
+    return true;
+}
+
+void uartTxMonitor(uartPort_t *s)
+{
+    uartDevice_t *uart = container_of(s, uartDevice_t, port);
+
+    if (uart->txPinState == TX_PIN_ACTIVE) {
+        IO_t txIO = IOGetByTag(uart->tx.pin);
+
+        // Disable the UART transmitter
+        CLEAR_BIT(s->USARTx->CR1, USART_CR1_TE);
+
+        // Switch TX to an input with pullup so it's state can be monitored
+        uart->txPinState = TX_PIN_MONITOR;
+        IOConfigGPIO(txIO, IOCFG_IPU);
+    }
+}
+
 static void handleUsartTxDma(uartPort_t *s)
 {
+    uartDevice_t *uart = container_of(s, uartDevice_t, port);
+
     uartTryStartTxDMA(s);
+
+    if (s->txDMAEmpty && (uart->txPinState != TX_PIN_IGNORE)) {
+        // Switch TX to an input with pullup so it's state can be monitored
+        uartTxMonitor(s);
+    }
 }
 
 void uartDmaIrqHandler(dmaChannelDescriptor_t* descriptor)
@@ -268,6 +318,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
 
     s->USARTx = hardware->reg;
 
+    s->checkUsartTxOutput = checkUsartTxOutput;
+
 #ifdef USE_DMA
     uartConfigureDma(uart);
 #endif
@@ -279,13 +331,22 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
         RCC_ClockCmd(hardware->rcc, ENABLE);
     }
 
+    uart->txPinState = TX_PIN_IGNORE;
+
     if (options & SERIAL_BIDIR) {
         IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
         IOConfigGPIOAF(txIO, ((options & SERIAL_BIDIR_PP) || (options & SERIAL_BIDIR_PP_PD)) ? IOCFG_AF_PP : IOCFG_AF_OD, hardware->af);
     } else {
         if ((mode & MODE_TX) && txIO) {
             IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
-            IOConfigGPIOAF(txIO, IOCFG_AF_PP_UP, hardware->af);
+
+            if (options & SERIAL_CHECK_TX) {
+                uart->txPinState = TX_PIN_ACTIVE;
+                // Switch TX to an input with pullup so it's state can be monitored
+                uartTxMonitor(s);
+            } else {
+                IOConfigGPIOAF(txIO, IOCFG_AF_PP_UP, hardware->af);
+            }
         }
 
         if ((mode & MODE_RX) && rxIO) {
@@ -320,6 +381,14 @@ void uartIrqHandler(uartPort_t *s)
         }
     }
 
+    // Detect completion of transmission
+    if (USART_GetITStatus(s->USARTx, USART_IT_TC) == SET) {
+        // Switch TX to an input with pullup so it's state can be monitored
+        uartTxMonitor(s);
+
+        USART_ClearITPendingBit(s->USARTx, USART_IT_TC);
+    }
+
     if (!s->txDMAResource && (USART_GetITStatus(s->USARTx, USART_IT_TXE) == SET)) {
         if (s->port.txBufferTail != s->port.txBufferHead) {
             USART_SendData(s->USARTx, s->port.txBuffer[s->port.txBufferTail]);
diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c
index ef3c81daad5..a63ff201d20 100644
--- a/src/main/drivers/serial_uart_stm32f7xx.c
+++ b/src/main/drivers/serial_uart_stm32f7xx.c
@@ -345,6 +345,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
     s->port.rxBufferSize = hardware->rxBufferSize;
     s->port.txBufferSize = hardware->txBufferSize;
 
+    s->checkUsartTxOutput = checkUsartTxOutput;
+
 #ifdef USE_DMA
     uartConfigureDma(uartdev);
 #endif
@@ -358,6 +360,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
     IO_t txIO = IOGetByTag(uartdev->tx.pin);
     IO_t rxIO = IOGetByTag(uartdev->rx.pin);
 
+    uartdev->txPinState = TX_PIN_IGNORE;
+
     if ((options & SERIAL_BIDIR) && txIO) {
         ioConfig_t ioCfg = IO_CONFIG(
             ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP) || (options & SERIAL_BIDIR_PP_PD)) ? GPIO_MODE_AF_PP : GPIO_MODE_AF_OD,
@@ -371,7 +375,14 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
     else {
         if ((mode & MODE_TX) && txIO) {
             IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
-            IOConfigGPIOAF(txIO, IOCFG_AF_PP, uartdev->tx.af);
+
+            if (options & SERIAL_CHECK_TX) {
+                uartdev->txPinState = TX_PIN_MONITOR;
+                // Switch TX to UART output whilst UART sends idle preamble
+                checkUsartTxOutput(s);
+            } else {
+                IOConfigGPIOAF(txIO, IOCFG_AF_PP, uartdev->tx.af);
+            }
         }
 
         if ((mode & MODE_RX) && rxIO) {
diff --git a/src/main/drivers/serial_uart_stm32g4xx.c b/src/main/drivers/serial_uart_stm32g4xx.c
index 2c9197692e0..1a75f164936 100644
--- a/src/main/drivers/serial_uart_stm32g4xx.c
+++ b/src/main/drivers/serial_uart_stm32g4xx.c
@@ -279,6 +279,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
     s->port.rxBufferSize = hardware->rxBufferSize;
     s->port.txBufferSize = hardware->txBufferSize;
 
+    s->checkUsartTxOutput = checkUsartTxOutput;
+
 #ifdef USE_DMA
     uartConfigureDma(uartdev);
 #endif
@@ -292,6 +294,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
     IO_t txIO = IOGetByTag(uartdev->tx.pin);
     IO_t rxIO = IOGetByTag(uartdev->rx.pin);
 
+    uartdev->txPinState = TX_PIN_IGNORE;
+
     if ((options & SERIAL_BIDIR) && txIO) {
         ioConfig_t ioCfg = IO_CONFIG(
             ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP) || (options & SERIAL_BIDIR_PP_PD)) ? GPIO_MODE_AF_PP : GPIO_MODE_AF_OD,
@@ -304,7 +308,14 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
     } else {
         if ((mode & MODE_TX) && txIO) {
             IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
-            IOConfigGPIOAF(txIO, IOCFG_AF_PP, uartdev->tx.af);
+
+            if (options & SERIAL_CHECK_TX) {
+                uartdev->txPinState = TX_PIN_ACTIVE;
+                // Switch TX to an input with pullup so it's state can be monitored
+                uartTxMonitor(s);
+            } else {
+                IOConfigGPIOAF(txIO, IOCFG_AF_PP, uartdev->tx.af);
+            }
         }
 
         if ((mode & MODE_RX) && rxIO) {
diff --git a/src/main/drivers/serial_uart_stm32h7xx.c b/src/main/drivers/serial_uart_stm32h7xx.c
index 09011bb76a9..c21a908d4d5 100644
--- a/src/main/drivers/serial_uart_stm32h7xx.c
+++ b/src/main/drivers/serial_uart_stm32h7xx.c
@@ -456,6 +456,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
     s->port.rxBufferSize = hardware->rxBufferSize;
     s->port.txBufferSize = hardware->txBufferSize;
 
+    s->checkUsartTxOutput = checkUsartTxOutput;
+
 #ifdef USE_DMA
     uartConfigureDma(uartdev);
 #endif
@@ -469,6 +471,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
     IO_t txIO = IOGetByTag(uartdev->tx.pin);
     IO_t rxIO = IOGetByTag(uartdev->rx.pin);
 
+    uartdev->txPinState = TX_PIN_IGNORE;
+
     if ((options & SERIAL_BIDIR) && txIO) {
         ioConfig_t ioCfg = IO_CONFIG(
             ((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP) || (options & SERIAL_BIDIR_PP_PD)) ? GPIO_MODE_AF_PP : GPIO_MODE_AF_OD,
@@ -481,7 +485,14 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
     } else {
         if ((mode & MODE_TX) && txIO) {
             IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
-            IOConfigGPIOAF(txIO, IOCFG_AF_PP, uartdev->tx.af);
+
+            if (options & SERIAL_CHECK_TX) {
+                uartdev->txPinState = TX_PIN_ACTIVE;
+                // Switch TX to an input with pullup so it's state can be monitored
+                uartTxMonitor(s);
+            } else {
+                IOConfigGPIOAF(txIO, IOCFG_AF_PP, uartdev->tx.af);
+            }
         }
 
         if ((mode & MODE_RX) && rxIO) {
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index b17522d848a..ec16015e1ee 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -103,7 +103,7 @@ void SysTick_Handler(void)
 
 // Return system uptime in microseconds (rollover in 70minutes)
 
-uint32_t microsISR(void)
+MMFLASH_CODE_NOINLINE uint32_t microsISR(void)
 {
     register uint32_t ms, pending, cycle_cnt;
 
@@ -262,7 +262,7 @@ void failureMode(failureMode_e mode)
 void initialiseMemorySections(void)
 {
 #ifdef USE_ITCM_RAM
-    /* Load functions into ITCM RAM */
+    /* Load fast-functions into ITCM RAM */
     extern uint8_t tcm_code_start;
     extern uint8_t tcm_code_end;
     extern uint8_t tcm_code;
@@ -284,6 +284,15 @@ void initialiseMemorySections(void)
     extern uint8_t _sfastram_idata;
     memcpy(&_sfastram_data, &_sfastram_idata, (size_t) (&_efastram_data - &_sfastram_data));
 #endif
+
+#ifdef USE_RAM_CODE
+    /* Load slow-functions into ITCM RAM */
+    extern uint8_t ram_code_start;
+    extern uint8_t ram_code_end;
+    extern uint8_t ram_code;
+    memcpy(&ram_code_start, &ram_code, (size_t) (&ram_code_end - &ram_code_start));
+#endif
+
 }
 
 #ifdef STM32H7
diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h
index 27609283840..21b1719c67b 100644
--- a/src/main/drivers/system.h
+++ b/src/main/drivers/system.h
@@ -73,6 +73,10 @@ uint32_t getCycleCounter(void);
 void systemProcessResetReason(void);
 #endif
 
+// memory
+void memoryMappedModeInit(void);
+bool isMemoryMappedModeEnabledOnBoot(void);
+
 void initialiseMemorySections(void);
 #ifdef STM32H7
 void initialiseD2MemorySections(void);
diff --git a/src/main/drivers/system_stm32g4xx.c b/src/main/drivers/system_stm32g4xx.c
index d9778728ec2..b2e067947b6 100644
--- a/src/main/drivers/system_stm32g4xx.c
+++ b/src/main/drivers/system_stm32g4xx.c
@@ -86,30 +86,55 @@ void systemReset(void)
 
 void systemResetToBootloader(bootloaderRequestType_e requestType)
 {
-    UNUSED(requestType);
+    switch (requestType) {
+    case BOOTLOADER_REQUEST_ROM:
+    default:
+        persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
 
-    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
+        break;
+    }
 
     __disable_irq();
     NVIC_SystemReset();
 }
 
 #define SYSMEMBOOT_VECTOR_TABLE ((uint32_t *)0x1fff0000)
+#define SYSMEMBOOT_LOADER       ((uint32_t *)0x1fff0000)
 
 typedef void *(*bootJumpPtr)(void);
 
+typedef void resetHandler_t(void);
+
+typedef struct isrVector_s {
+    __I uint32_t    stackEnd;
+    resetHandler_t *resetHandler;
+} isrVector_t;
+
 void systemJumpToBootloader(void)
-{   
-    __SYSCFG_CLK_ENABLE();
-    
-    uint32_t bootStack =  SYSMEMBOOT_VECTOR_TABLE[0];
-    
+{
+    //DeInit all used peripherals
+    HAL_RCC_DeInit();
+
+    //Disable all system timers and set to default values
+    SysTick->CTRL = 0;
+    SysTick->LOAD = 0;
+    SysTick->VAL = 0;
+
+    //Disable all interrupts
+    __disable_irq();
+
+    //remap system memory
+    __HAL_SYSCFG_REMAPMEMORY_SYSTEMFLASH();
+
+    //default bootloader call stack routine
+    uint32_t bootStack = SYSMEMBOOT_VECTOR_TABLE[0];
+
     bootJumpPtr SysMemBootJump = (bootJumpPtr)SYSMEMBOOT_VECTOR_TABLE[1];
-    
+
     __set_MSP(bootStack); //Set the main stack pointer to its default values
-    
+
     SysMemBootJump();
-    
+
     while (1);
 }
 
diff --git a/src/main/drivers/system_stm32h7xx.c b/src/main/drivers/system_stm32h7xx.c
index 49f96f831be..8f4f8ce79b9 100644
--- a/src/main/drivers/system_stm32h7xx.c
+++ b/src/main/drivers/system_stm32h7xx.c
@@ -62,6 +62,41 @@ bool isMPUSoftReset(void)
         return false;
 }
 
+#if defined(USE_FLASH_MEMORY_MAPPED)
+
+/*
+ * Memory mapped targets use a bootloader which enables memory mapped mode before running the firmware directly from external flash.
+ * Code running from external flash, i.e. most of the firmware, must not disable peripherals or reconfigure pins used by the CPU to access the flash chip.
+ * Refer to reference manuals and linker scripts for addresses of memory mapped regions.
+ * STM32H830 - RM0468 "Table 6. Memory map and default device memory area attributes"
+ *
+ * If the config is also stored on the same flash chip that code is running from then VERY special care must be taken when detecting the flash chip
+ * and when writing an updated config back to the flash.
+ */
+
+static bool memoryMappedModeEnabledOnBoot = false;
+
+bool isMemoryMappedModeEnabledOnBoot(void)
+{
+    return memoryMappedModeEnabledOnBoot;
+}
+
+void memoryMappedModeInit(void)
+{
+#if defined(STM32H730xx) || defined(STM32H723xx)
+    // Smaller MCU packages have ONE OCTOSPI interface which supports memory mapped mode.
+    memoryMappedModeEnabledOnBoot = READ_BIT(OCTOSPI1->CR, OCTOSPI_CR_FMODE) == OCTOSPI_CR_FMODE;
+#else
+#error No Memory Mapped implementation on current MCU.
+#endif
+}
+#else
+bool isMemoryMappedModeEnabledOnBoot(void)
+{
+    return false;
+}
+#endif
+
 void systemInit(void)
 {
     // Configure NVIC preempt/priority groups
diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index 1010285e593..3445aa55433 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -495,7 +495,7 @@ void tryArm(void)
         const timeUs_t currentTimeUs = micros();
 
 #ifdef USE_DSHOT
-        if (currentTimeUs - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) {
+        if (cmpTimeUs(currentTimeUs, getLastDshotBeaconCommandTimeUs()) < DSHOT_BEACON_GUARD_DELAY_US) {
             if (tryingToArm == ARMING_DELAYED_DISARMED) {
                 if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
                     tryingToArm = ARMING_DELAYED_CRASHFLIP;
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index 3ab56faf261..f6aa8f59b1d 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -47,6 +47,7 @@
 #include "drivers/adc.h"
 #include "drivers/bus.h"
 #include "drivers/bus_i2c.h"
+#include "drivers/bus_octospi.h"
 #include "drivers/bus_quadspi.h"
 #include "drivers/bus_spi.h"
 #include "drivers/buttons.h"
@@ -195,8 +196,7 @@ void busSwitchInit(void)
 }
 #endif
 
-
-static void configureSPIAndQuadSPI(void)
+static void configureSPIBusses(void)
 {
 #ifdef USE_SPI
     spiPinConfigure(spiPinConfig(0));
@@ -226,7 +226,10 @@ static void configureSPIAndQuadSPI(void)
     spiInit(SPIDEV_6);
 #endif
 #endif // USE_SPI
+}
 
+static void configureQuadSPIBusses(void)
+{
 #ifdef USE_QUADSPI
     quadSpiPinConfigure(quadSpiConfig(0));
 
@@ -236,6 +239,15 @@ static void configureSPIAndQuadSPI(void)
 #endif // USE_QUAD_SPI
 }
 
+static void configureOctoSPIBusses(void)
+{
+#ifdef USE_OCTOSPI
+#ifdef USE_OCTOSPI_DEVICE_1
+    octoSpiInit(OCTOSPIDEV_1);
+#endif
+#endif
+}
+
 #ifdef USE_SDCARD
 static void sdCardAndFSInit(void)
 {
@@ -291,9 +303,10 @@ void init(void)
 #endif
 
     enum {
-        FLASH_INIT_ATTEMPTED            = (1 << 0),
-        SD_INIT_ATTEMPTED               = (1 << 1),
-        SPI_AND_QSPI_INIT_ATTEMPTED      = (1 << 2),
+        FLASH_INIT_ATTEMPTED                = (1 << 0),
+        SD_INIT_ATTEMPTED                   = (1 << 1),
+        SPI_BUSSES_INIT_ATTEMPTED           = (1 << 2),
+        QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED = (1 << 3),
     };
     uint8_t initFlags = 0;
 
@@ -303,19 +316,17 @@ void init(void)
     // Config in sdcard presents an issue with pin configuration since the pin and sdcard configs for the
     // sdcard are in the config which is on the sdcard which we can't read yet!
     //
-    // FIXME We need to add configuration somewhere, e.g. bootloader image or reserved flash area, that can be read by the firmware.
-    // it's currently possible for the firmware resource allocation to be wrong after the config is loaded if the user changes the settings.
-    // This would cause undefined behaviour once the config is loaded.  so for now, users must NOT change sdio/spi configs needed for
-    // the system to boot and/or to save the config.
+    // FIXME For now, users must NOT change flash/pin configs needed for the system to boot and/or to save the config.
+    // One possible solution is to lock the pins for the flash chip so they cannot be modified post-boot.
     //
-    // note that target specific SDCARD/SDIO/SPI/QUADSPI configs are
+    // note that target specific SDCARD/SDIO/SPI/QUADSPI/OCTOSPI configs are
     // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_SDCARD.
     //
 
     //
     // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called.
-    // Target designers must ensure other devices connected the same SPI/QUADSPI interface as the flash chip do not
-    // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI CS lines.
+    // Target designers must ensure other devices connected the same SPI/QUADSPI/OCTOSPI interface as the flash chip do not
+    // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI/OCTOSPI CS lines.
     //
 
 #ifdef TARGET_BUS_INIT
@@ -325,8 +336,8 @@ void init(void)
     pgResetAll();
 
 #ifdef USE_SDCARD_SPI
-    configureSPIAndQuadSPI();
-    initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
+    configureSPIBusses();
+    initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
 #endif
 
     sdCardAndFSInit();
@@ -346,37 +357,42 @@ void init(void)
 
 #endif // CONFIG_IN_SDCARD
 
-#ifdef CONFIG_IN_EXTERNAL_FLASH
+#if defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
     //
     // Config on external flash presents an issue with pin configuration since the pin and flash configs for the
     // external flash are in the config which is on a chip which we can't read yet!
     //
-    // FIXME We need to add configuration somewhere, e.g. bootloader image or reserved flash area, that can be read by the firmware.
-    // it's currently possible for the firmware resource allocation to be wrong after the config is loaded if the user changes the settings.
-    // This would cause undefined behaviour once the config is loaded.  so for now, users must NOT change flash/pin configs needed for
-    // the system to boot and/or to save the config.
+    // FIXME For now, users must NOT change flash/pin configs needed for the system to boot and/or to save the config.
+    // One possible solution is to lock the pins for the flash chip so they cannot be modified post-boot.
     //
-    // note that target specific FLASH/SPI/QUADSPI configs are
-    // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_EXTERNAL_FLASH.
+    // note that target specific FLASH/SPI/QUADSPI/OCTOSPI configs are
+    // also not supported in USE_TARGET_CONFIG/targetConfigure() when using CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH.
     //
 
     //
     // IMPORTANT: all default flash and pin configurations must be valid for the target after pgResetAll() is called.
-    // Target designers must ensure other devices connected the same SPI/QUADSPI interface as the flash chip do not
-    // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI CS lines.
+    // Target designers must ensure other devices connected the same SPI/QUADSPI/OCTOSPI interface as the flash chip do not
+    // cause communication issues with the flash chip.  e.g. use external pullups on SPI/QUADSPI/OCTOSPI CS lines.
     //
     pgResetAll();
 
 #ifdef TARGET_BUS_INIT
-#error "CONFIG_IN_EXTERNAL_FLASH and TARGET_BUS_INIT are mutually exclusive"
+#error "CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH and TARGET_BUS_INIT are mutually exclusive"
 #endif
 
-    configureSPIAndQuadSPI();
-    initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
+#if defined(CONFIG_IN_EXTERNAL_FLASH)
+    configureSPIBusses();
+    initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
+#endif
 
+#if defined(CONFIG_IN_MEMORY_MAPPED_FLASH) || defined(CONFIG_IN_EXTERNAL_FLASH)
+    configureQuadSPIBusses();
+    configureOctoSPIBusses();
+    initFlags |= QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED;
+#endif
 
 #ifndef USE_FLASH_CHIP
-#error "CONFIG_IN_EXTERNAL_FLASH requires USE_FLASH_CHIP to be defined."
+#error "CONFIG_IN_EXTERNAL_FLASH/CONFIG_IN_MEMORY_MAPPED_FLASH requires USE_FLASH_CHIP to be defined."
 #endif
 
     bool haveFlash = flashInit(flashConfig());
@@ -386,7 +402,8 @@ void init(void)
     }
     initFlags |= FLASH_INIT_ATTEMPTED;
 
-#endif // CONFIG_IN_EXTERNAL_FLASH
+#endif // CONFIG_IN_EXTERNAL_FLASH || CONFIG_IN_MEMORY_MAPPED_FLASH
+
 
     initEEPROM();
 
@@ -588,10 +605,16 @@ void init(void)
 
 #else
 
-    // Depending on compilation options SPI/QSPI initialisation may already be done.
-    if (!(initFlags & SPI_AND_QSPI_INIT_ATTEMPTED)) {
-        configureSPIAndQuadSPI();
-        initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
+    // Depending on compilation options SPI/QSPI/OSPI initialisation may already be done.
+    if (!(initFlags & SPI_BUSSES_INIT_ATTEMPTED)) {
+        configureSPIBusses();
+        initFlags |= SPI_BUSSES_INIT_ATTEMPTED;
+    }
+
+    if (!(initFlags & QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED)) {
+        configureQuadSPIBusses();
+        configureOctoSPIBusses();
+        initFlags |= QUAD_OCTO_SPI_BUSSES_INIT_ATTEMPTED;
     }
 
 #if defined(USE_SDCARD_SDIO) && !defined(CONFIG_IN_SDCARD) && defined(STM32H7)
diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h
index dab828d8de1..b87d784c509 100644
--- a/src/main/fc/parameter_names.h
+++ b/src/main/fc/parameter_names.h
@@ -142,12 +142,14 @@
 
 #define PARAM_NAME_GPS_RESCUE_RETURN_ALT "gps_rescue_return_alt"
 #define PARAM_NAME_GPS_RESCUE_RETURN_SPEED "gps_rescue_ground_speed"
-#define PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX "gps_rescue_pitch_angle_max"
+#define PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE "gps_rescue_max_angle"
 #define PARAM_NAME_GPS_RESCUE_ROLL_MIX "gps_rescue_roll_mix"
+#define PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF "gps_rescue_pitch_cutoff"
 
 #define PARAM_NAME_GPS_RESCUE_DESCENT_DIST "gps_rescue_descent_dist"
 #define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate"
 #define PARAM_NAME_GPS_RESCUE_LANDING_ALT "gps_rescue_landing_alt"
+#define PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD "gps_rescue_disarm_threshold"
 
 #define PARAM_NAME_GPS_RESCUE_THROTTLE_MIN "gps_rescue_throttle_min"
 #define PARAM_NAME_GPS_RESCUE_THROTTLE_MAX "gps_rescue_throttle_max"
diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index 0872abea86b..167df0e737f 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -177,10 +177,16 @@ void processRcStickPositions(void)
             resetTryingToArm();
             // Disarming via ARM BOX
             resetArmingDisabled();
-            const bool switchFailsafe = (failsafeIsActive() && (IS_RC_MODE_ACTIVE(BOXFAILSAFE) || IS_RC_MODE_ACTIVE(BOXGPSRESCUE)));
-            if (ARMING_FLAG(ARMED) && (failsafeIsReceivingRxData() || switchFailsafe)) {
+            const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
+            if (ARMING_FLAG(ARMED) && (failsafeIsReceivingRxData() || boxFailsafeSwitchIsOn)) {
+                // in a true signal loss situation, allow disarm only once we regain validated RxData (failsafeIsReceivingRxData = true),
+                // to avoid potentially false disarm signals soon after link recover
+                // Note that BOXFAILSAFE will also drive failsafeIsReceivingRxData false (immediately at start or end)
+                // That's why we explicitly allow disarm here BOXFAILSAFE switch is active
+                // Note that BOXGPSRESCUE mode does not trigger failsafe - we can always disarm in that mode
                 rcDisarmTicks++;
                 if (rcDisarmTicks > 3) {
+                    // require three duplicate disarm values in a row before we disarm
                     disarm(DISARM_REASON_SWITCH);
                 }
             }
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 94aa551e186..d1a9aac1cc0 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -104,7 +104,7 @@ void failsafeReset(void)
     failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
     failsafeState.phase = FAILSAFE_IDLE;
     failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
-    failsafeState.failsafeSwitchWasOn = false;
+    failsafeState.boxFailsafeSwitchWasOn = false;
 }
 
 void failsafeInit(void)
@@ -125,7 +125,7 @@ bool failsafeIsMonitoring(void)
     return failsafeState.monitoring;
 }
 
-bool failsafeIsActive(void) // real or switch-induced stage 2 failsafe
+bool failsafeIsActive(void) // real or BOXFAILSAFE induced stage 2 failsafe is currently active
 {
     return failsafeState.active;
 }
@@ -143,9 +143,9 @@ static bool failsafeShouldHaveCausedLandingByNow(void)
 bool failsafeIsReceivingRxData(void)
 {
     return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
-    // False with failsafe switch or when no valid packets for 100ms or any flight channel invalid for 300ms,
-    // stays false until after recovery period expires
-    // Link down is the trigger for the various failsafe stage 2 outcomes.
+    // False with BOXFAILSAFE switch or when no valid packets for 100ms or any flight channel invalid for 300ms,
+    // becomes true immediately BOXFAILSAFE switch reverts, or after recovery period expires when valid packets are received
+    // rxLinkState RXLINK_DOWN (not up) is the trigger for the various failsafe stage 2 outcomes.
 }
 
 void failsafeOnRxSuspend(uint32_t usSuspendPeriod)
@@ -160,7 +160,10 @@ void failsafeOnRxResume(void)
 }
 
 void failsafeOnValidDataReceived(void)
-// runs when packets are received for more than the signal validation period (100ms)
+// enters stage 2
+// runs, after prior a signal loss, immediately when packets are received or the BOXFAILSAFE switch is reverted
+// rxLinkState will go RXLINK_UP immediately if BOXFAILSAFE goes back ON since receivingRxDataPeriodPreset is set to zero in that case
+// otherwise RXLINK_UP is delayed for the recovery period (failsafe_recovery_delay, default 1s, 0-20, min 0.2s)
 {
     unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     // clear RXLOSS in OSD immediately we get a good packet, and un-set its arming block
@@ -188,14 +191,16 @@ void failsafeOnValidDataReceived(void)
 }
 
 void failsafeOnValidDataFailed(void)
-// runs when packets are lost for more than the signal validation period (100ms)
+// run from rc.c when packets are lost for more than the signal validation period (100ms), or immediately BOXFAILSAFE switch is active
+// after the stage 1 delay has expired, sets the rxLinkState to RXLINK_DOWN, ie not up, causing failsafeIsReceivingRxData to become false
+// if failsafe is configured to go direct to stage 2, this is emulated immediately in failsafeUpdateState()
 {
     setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     //  set RXLOSS in OSD and block arming after 100ms of signal loss (is restored in rx.c immediately signal returns)
 
     failsafeState.validRxDataFailedAt = millis();
     if ((cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod)) {
-        // sets rxLinkState = DOWN to initiate stage 2 failsafe, if no validated signal for the stage 1 period
+        // sets rxLinkState = DOWN to initiate stage 2 failsafe
         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
         // show RXLOSS and block arming
     }
@@ -225,19 +230,18 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
     }
 
     bool receivingRxData = failsafeIsReceivingRxData();
-    // returns state of FAILSAFE_RXLINK_UP
-    // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived, after the various Stage 1 and recovery delays
-    // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour
+    // returns state of FAILSAFE_RXLINK_UP, which 
+    // goes false after the stage 1 delay, whether from signal loss or BOXFAILSAFE switch activation
+    // goes true immediately BOXFAILSAFE switch is reverted, or after recovery delay once signal recovers
+    // essentially means 'should be in failsafe stage 2'
 
     DEBUG_SET(DEBUG_FAILSAFE, 2, receivingRxData); // from Rx alone, not considering switch
 
     bool armed = ARMING_FLAG(ARMED);
-    bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
     beeperMode_e beeperMode = BEEPER_SILENCE;
 
-    if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
-        // Aux switch set to failsafe stage2 emulates immediate loss of signal without waiting
-        failsafeOnValidDataFailed();
+    if (IS_RC_MODE_ACTIVE(BOXFAILSAFE) && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
+        // Force immediate stage 2 responses if mode is failsafe stage2 to emulate immediate loss of signal without waiting
         receivingRxData = false;
     }
 
@@ -253,13 +257,14 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
 
         switch (failsafeState.phase) {
             case FAILSAFE_IDLE:
-                failsafeState.failsafeSwitchWasOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
+                failsafeState.boxFailsafeSwitchWasOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
+                // store and use the switch state as it was at the start of the failsafe
                 if (armed) {
                     // Track throttle command below minimum time
                     if (calculateThrottleStatus() != THROTTLE_LOW) {
                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
                     }
-                    if (failsafeState.failsafeSwitchWasOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
+                    if (failsafeState.boxFailsafeSwitchWasOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
                         // Failsafe switch is configured as KILL switch and is switched ON
                         failsafeState.active = true;
                         failsafeState.events++;
@@ -291,7 +296,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                     }
                 } else {
                     // When NOT armed, enable failsafe mode to show warnings in OSD
-                    if (failsafeState.failsafeSwitchWasOn) {
+                    if (failsafeState.boxFailsafeSwitchWasOn) {
                         ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
                     } else {
                         DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
@@ -327,7 +332,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                             break;
 #endif
                     }
-                    if (failsafeState.failsafeSwitchWasOn) {
+                    if (failsafeState.boxFailsafeSwitchWasOn) {
                         failsafeState.receivingRxDataPeriodPreset = 0;
                         // recover immediately if failsafe was triggered by a switch
                     } else {
@@ -359,7 +364,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
 #ifdef USE_GPS_RESCUE
             case FAILSAFE_GPS_RESCUE:
                 if (receivingRxData) {
-                    if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || failsafeState.failsafeSwitchWasOn) {
+                    if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || failsafeState.boxFailsafeSwitchWasOn) {
                         // exits the rescue immediately if failsafe was initiated by switch, otherwise 
                         // requires stick input to exit the rescue after a true Rx loss failsafe
                         // NB this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale
@@ -418,7 +423,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                 break;
         }
 
-    DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.failsafeSwitchWasOn);
+    DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.boxFailsafeSwitchWasOn);
     DEBUG_SET(DEBUG_FAILSAFE, 3, failsafeState.phase);
 
     } while (reprocessState);
diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
index 350c9e2df26..959ab9644f4 100644
--- a/src/main/flight/failsafe.h
+++ b/src/main/flight/failsafe.h
@@ -90,7 +90,7 @@ typedef struct failsafeState_s {
     uint32_t receivingRxDataPeriodPreset;   // preset for the required period of valid rxData
     failsafePhase_e phase;
     failsafeRxLinkState_e rxLinkState;
-    bool failsafeSwitchWasOn;
+    bool boxFailsafeSwitchWasOn;
 } failsafeState_t;
 
 void failsafeInit(void);
diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c
index b8e44b4e3ca..dd4126f470a 100644
--- a/src/main/flight/gps_rescue.c
+++ b/src/main/flight/gps_rescue.c
@@ -85,10 +85,14 @@ typedef struct {
     float rollAngleLimitDeg;
     float descentDistanceM;
     int8_t secondsFailing;
-    float altitudeStep;
-    float descentRateModifier;
+    float throttleDMultiplier;
     float yawAttenuator;
     float disarmThreshold;
+    float velocityITermAccumulator;
+    float velocityPidCutoff;
+    float velocityPidCutoffModifier;
+    float velocityItermAttenuator;
+    float velocityItermRelax;
 } rescueIntent_s;
 
 typedef struct {
@@ -102,10 +106,13 @@ typedef struct {
     float errorAngle;
     float gpsDataIntervalSeconds;
     float altitudeDataIntervalSeconds;
+    float gpsRescueTaskIntervalSeconds;
     float velocityToHomeCmS;
     float alitutudeStepCm;
     float maxPitchStep;
     float absErrorAngle;
+    float groundspeedPitchAttenuator;
+    float imuYawCogGain;
 } rescueSensorData_s;
 
 typedef struct {
@@ -117,11 +124,7 @@ typedef struct {
 } rescueState_s;
 
 #define GPS_RESCUE_MAX_YAW_RATE          180    // deg/sec max yaw rate
-#define GPS_RESCUE_MIN_DESCENT_DIST_M    5      // minimum descent distance
-#define GPS_RESCUE_MAX_ITERM_VELOCITY    1000   // max iterm value for velocity
-#define GPS_RESCUE_MAX_ITERM_THROTTLE    200    // max iterm value for throttle
-#define GPS_RESCUE_MAX_PITCH_RATE        3000   // max change in pitch per second in degrees * 100
-#define GPS_RESCUE_DISARM_THRESHOLD      2.0f   // disarm threshold in G's
+#define GPS_RESCUE_MAX_THROTTLE_ITERM    200    // max iterm value for throttle in degrees * 100
 
 static float rescueThrottle;
 static float rescueYaw;
@@ -129,27 +132,29 @@ float       gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
 bool        magForceDisable = false;
 static bool newGPSData = false;
 static pt2Filter_t throttleDLpf;
-static pt2Filter_t velocityDLpf;
-static pt3Filter_t pitchLpf;
+static pt1Filter_t velocityDLpf;
+static pt3Filter_t velocityUpsampleLpf;
 
 rescueState_s rescueState;
 
 void gpsRescueInit(void)
 {
-    const float sampleTimeS = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ);
-    float cutoffHz, gain;
+    rescueState.sensor.gpsRescueTaskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ);
 
+    float cutoffHz, gain;
     cutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
-    gain = pt2FilterGain(cutoffHz, sampleTimeS);
+    gain = pt2FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
     pt2FilterInit(&throttleDLpf, gain);
 
-    cutoffHz = 0.8f;
-    gain = pt2FilterGain(cutoffHz, 1.0f);
-    pt2FilterInit(&velocityDLpf, gain);
+    cutoffHz = gpsRescueConfig()->pitchCutoffHz / 100.0f;
+    rescueState.intent.velocityPidCutoff = cutoffHz;
+    rescueState.intent.velocityPidCutoffModifier = 1.0f;
+    gain = pt1FilterGain(cutoffHz, 1.0f);
+    pt1FilterInit(&velocityDLpf, gain);
 
-    cutoffHz = 4.0f;
-    gain = pt3FilterGain(cutoffHz, sampleTimeS);
-    pt3FilterInit(&pitchLpf, gain);
+    cutoffHz *= 4.0f; 
+    gain = pt3FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
+    pt3FilterInit(&velocityUpsampleLpf, gain);
 }
 
 /*
@@ -186,8 +191,10 @@ static void setReturnAltitude(void)
         // set the target altitude to current values, so there will be no D kick on first run
         rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm;
 
-        // Keep the descent distance and intended altitude up to date with latest GPS values
-        rescueState.intent.descentDistanceM = constrainf(rescueState.sensor.distanceToHomeM, GPS_RESCUE_MIN_DESCENT_DIST_M, gpsRescueConfig()->descentDistanceM);
+        // Intended descent distance for rescues that start outside the minRescueDth distance
+        // Set this to the user's intended descent distance, but not more than half the distance to home to ensure some fly home time
+        rescueState.intent.descentDistanceM = fminf(0.5f * rescueState.sensor.distanceToHomeM, gpsRescueConfig()->descentDistanceM);
+ 
         const float initialAltitudeCm = gpsRescueConfig()->initialAltitudeM * 100.0f;
         const float rescueAltitudeBufferCm = gpsRescueConfig()->rescueAltitudeBufferM * 100.0f;
         switch (gpsRescueConfig()->altitudeMode) {
@@ -210,7 +217,6 @@ static void rescueAttainPosition(void)
     // runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase.
     static float previousVelocityError = 0.0f;
     static float velocityI = 0.0f;
-    static float previousPitchAdjustment = 0.0f;
     static float throttleI = 0.0f;
     static float previousAltitudeError = 0.0f;
     static int16_t throttleAdjustment = 0;
@@ -226,11 +232,11 @@ static void rescueAttainPosition(void)
         // Initialize internal variables each time GPS Rescue is started
         previousVelocityError = 0.0f;
         velocityI = 0.0f;
-        previousPitchAdjustment = 0.0f;
         throttleI = 0.0f;
         previousAltitudeError = 0.0f;
         throttleAdjustment = 0;
-        rescueState.intent.disarmThreshold = GPS_RESCUE_DISARM_THRESHOLD;
+        rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold * 0.1f;
+        rescueState.sensor.imuYawCogGain = 1.0f;
         return;
     case RESCUE_DO_NOTHING:
         // 20s of slow descent for switch induced sanity failures to allow time to recover
@@ -246,7 +252,7 @@ static void rescueAttainPosition(void)
         Altitude (throttle) controller
     */
     // currentAltitudeCm is updated at TASK_GPS_RESCUE_RATE_HZ
-    const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) * 0.01f;
+    const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100.0f;
     // height above target in metres (negative means too low)
     // at the start, the target starts at current altitude plus one step.  Increases stepwise to intended value.
 
@@ -255,20 +261,18 @@ static void rescueAttainPosition(void)
 
     // I component
     throttleI += 0.1f * gpsRescueConfig()->throttleI * altitudeError * rescueState.sensor.altitudeDataIntervalSeconds;
-    throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE, 1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE);
+    throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM, 1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM);
     // up to 20% increase in throttle from I alone
 
     // D component is error based, so includes positive boost when climbing and negative boost on descent
-    float verticalSpeed = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds);
+    float throttleD = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds);
     previousAltitudeError = altitudeError;
-    verticalSpeed += rescueState.intent.descentRateModifier * verticalSpeed;
-    // add up to 2x D when descent rate is faster
-
-    float throttleD = pt2FilterApply(&throttleDLpf, verticalSpeed);
-
-    rescueState.intent.disarmThreshold = GPS_RESCUE_DISARM_THRESHOLD - throttleD / 15.0f; // make disarm more likely if throttle D is high
-
-    throttleD = gpsRescueConfig()->throttleD * throttleD;
+    // increase by up to 2x when descent rate is faster
+    throttleD *= rescueState.intent.throttleDMultiplier;
+    // apply user's throttle D gain
+    throttleD *= gpsRescueConfig()->throttleD;
+    // smooth
+    throttleD = pt2FilterApply(&throttleDLpf, throttleD);
 
     // acceleration component not currently implemented - was needed previously due to GPS lag, maybe not needed now.
 
@@ -294,12 +298,13 @@ static void rescueAttainPosition(void)
     // if the course over ground, due to wind or pre-exiting movement, is different from the attitude of the quad, the GPS correction will be less accurate
     // the craft should not return much less than 5m/s during the rescue or the GPS corrections may be inaccurate.
     // the faster the return speed, the more accurate the IMU will be, but the consequences of IMU error at the start are greater
-    // A compass (magnetometer) is vital for accurate GPS rescue at slow speeds, but must be calibrated and validated.
-
-    rescueYaw = rescueState.sensor.errorAngle * gpsRescueConfig()->yawP * rescueState.intent.yawAttenuator * 0.1f;
+    // A compass (magnetometer) is vital for accurate GPS rescue at slow speeds, but must be calibrated and validated
+    // WARNING:  Some GPS units give false Home values!  Always check the arrow points to home on leaving home.
+    rescueYaw = rescueState.sensor.errorAngle * gpsRescueConfig()->yawP * rescueState.intent.yawAttenuator / 10.0f;
     rescueYaw = constrainf(rescueYaw, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
     // rescueYaw is the yaw rate in deg/s to correct the heading error
 
+    // *** mix in some roll.  very important for heading tracking, since a yaw rate means the quad has drifted sideways
     const float rollMixAttenuator = constrainf(1.0f - fabsf(rescueYaw) * 0.01f, 0.0f, 1.0f);
     // less roll at higher yaw rates, no roll at 100 deg/s of yaw
     const float rollAdjustment = -rescueYaw * gpsRescueConfig()->rollMix * rollMixAttenuator;
@@ -307,12 +312,12 @@ static void rescueAttainPosition(void)
     // when gpsRescueConfig()->rollMix is zero, there is no roll adjustment
     // rollAdjustment is degrees * 100
     // note that the roll element has the opposite sign to the yaw element *before* GET_DIRECTION
-
     const float rollLimit = 100.0f * rescueState.intent.rollAngleLimitDeg;
     gpsRescueAngle[AI_ROLL] = constrainf(rollAdjustment, -rollLimit, rollLimit);
     // gpsRescueAngle is added to the normal roll Angle Mode corrections in pid.c
 
     rescueYaw *= GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
+    // rescueYaw is the yaw rate in deg/s to correct the heading error
 
     /**
         Pitch / velocity controller
@@ -322,51 +327,60 @@ static void rescueAttainPosition(void)
 
         const float sampleIntervalNormaliseFactor = rescueState.sensor.gpsDataIntervalSeconds * 10.0f;
 
-        const float velocityError = (rescueState.intent.targetVelocityCmS - rescueState.sensor.velocityToHomeCmS);
+        const float velocityError = rescueState.intent.targetVelocityCmS - rescueState.sensor.velocityToHomeCmS;
         // velocityError is in cm per second, positive means too slow.
         // NB positive pitch setpoint means nose down.
+        // target velocity can be very negative leading to large error before the start, with overshoot
 
         // P component
         const float velocityP = velocityError * gpsRescueConfig()->velP;
 
         // I component
-        velocityI += 0.01f * gpsRescueConfig()->velI * velocityError * sampleIntervalNormaliseFactor;
-        // increase amount added when GPS sample rate is slower
-        velocityI = constrainf(velocityI, -1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY, 1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY);
-        // I component alone cannot exceed a pitch angle of 10%
+        velocityI += 0.01f * gpsRescueConfig()->velI * velocityError * sampleIntervalNormaliseFactor * rescueState.intent.velocityItermRelax;
+        // velocityItermRelax is a time-based factor, 0->1 with time constant of 1s from when we start to fly home
+        // avoids excess iTerm accumulation during the initial acceleration phase and during descent.
+
+        // force iTerm to zero, to minimise overshoot during deceleration with descent
+        // and because if we over-fly the home point, we need to re-accumulate from zero, not the previously accumulated value
+        velocityI *= rescueState.intent.velocityItermAttenuator;
+
+        const float pitchAngleLimit = rescueState.intent.pitchAngleLimitDeg * 100.0f;
+        const float velocityILimit = 0.5f * pitchAngleLimit;
+        // I component alone cannot exceed half the max pitch angle
+        velocityI = constrainf(velocityI, -velocityILimit, velocityILimit);
 
         // D component
         float velocityD = ((velocityError - previousVelocityError) / sampleIntervalNormaliseFactor);
         previousVelocityError = velocityError;
-        const float gain = pt2FilterGain(0.8f, HZ_TO_INTERVAL(gpsGetSampleRateHz()));
-        pt2FilterUpdateCutoff(&velocityDLpf, gain);
-        velocityD = pt2FilterApply(&velocityDLpf, velocityD);
         velocityD *= gpsRescueConfig()->velD;
 
-        const float velocityIAttenuator = rescueState.intent.targetVelocityCmS / gpsRescueConfig()->rescueGroundspeed;
-        // reduces iTerm as target velocity decreases, to minimise overshoot during deceleration to landing phase
+        // smooth the D steps
+        const float cutoffHz = rescueState.intent.velocityPidCutoff * rescueState.intent.velocityPidCutoffModifier;
+        // note that this cutoff is increased up to 2x as we get closer to landing point in descend()
+        const float gain = pt1FilterGain(cutoffHz, rescueState.sensor.gpsDataIntervalSeconds);
+        pt1FilterUpdateCutoff(&velocityDLpf, gain);
+        velocityD = pt1FilterApply(&velocityDLpf, velocityD);
 
-        pitchAdjustment = velocityP + velocityD;
-        if (rescueState.phase == RESCUE_FLY_HOME) {
-            pitchAdjustment *= 0.7f; // attenuate pitch PIDs during main fly home phase, tighten up in descent.
-        }
-        pitchAdjustment += velocityI * velocityIAttenuator;
+        pitchAdjustment = velocityP + velocityI + velocityD;
+        // limit to maximum allowed angle
+        pitchAdjustment = constrainf(pitchAdjustment, -pitchAngleLimit, pitchAngleLimit);
 
-        const float movingAvgPitchAdjustment = 0.5f * (previousPitchAdjustment + pitchAdjustment);
-         // moving average seems to work best here, a lot of sequential up and down in velocity data
-        previousPitchAdjustment = pitchAdjustment;
-        pitchAdjustment = movingAvgPitchAdjustment;
         // pitchAdjustment is the absolute Pitch angle adjustment value in degrees * 100
         // it gets added to the normal level mode Pitch adjustments in pid.c
         DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 0, lrintf(velocityP));
         DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, lrintf(velocityD));
     }
 
-    const float pitchAdjustmentFiltered = pt3FilterApply(&pitchLpf, pitchAdjustment);
+    // if initiated too close, and in the climb phase, pitch forward in whatever direction the nose is oriented until min distance away
+    // intent is to get far enough away that, with an IMU error, the quad will have enough distance to home to correct that error in the fly home phase
+    if ((rescueState.phase == RESCUE_ATTAIN_ALT) && (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth)) {
+        pitchAdjustment = 1500.0f; // 15 degree pitch forward
+    }
+
     // upsampling and smoothing of pitch angle steps
+    float pitchAdjustmentFiltered = pt3FilterApply(&velocityUpsampleLpf, pitchAdjustment);
 
-    const float pitchAngleLimit = rescueState.intent.pitchAngleLimitDeg * 100.0f;
-    gpsRescueAngle[AI_PITCH] = constrainf(pitchAdjustmentFiltered, -pitchAngleLimit, pitchAngleLimit);
+    gpsRescueAngle[AI_PITCH] = pitchAdjustmentFiltered;
     // this angle gets added to the normal pitch Angle Mode control values in pid.c - will be seen in pitch setpoint
 
     DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 3, lrintf(rescueState.intent.targetVelocityCmS));
@@ -447,9 +461,9 @@ static void performSanityChecks(void)
     // if the quad is stuck, or if GPS data packets stop, there will be no change in distance to home
     // we can't use rescueState.sensor.currentVelocity because it will be held at the last good value if GPS data updates stop
     if (rescueState.phase == RESCUE_FLY_HOME) {
-        const float velocityToHomeCmS = previousDistanceToHomeCm- rescueState.sensor.distanceToHomeCm; // cm/s
+        const float velocityToHomeCmS = previousDistanceToHomeCm - rescueState.sensor.distanceToHomeCm; // cm/s
         previousDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
-        rescueState.intent.secondsFailing += (velocityToHomeCmS < 0.5f * rescueState.intent.targetVelocityCmS) ? 1 : -1;
+        rescueState.intent.secondsFailing += (velocityToHomeCmS < 0.1f * rescueState.intent.targetVelocityCmS) ? 1 : -1;
         rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 15);
         if (rescueState.intent.secondsFailing == 15) {
 #ifdef USE_MAG
@@ -473,7 +487,6 @@ static void performSanityChecks(void)
         rescueState.failure = RESCUE_LOWSATS;
     }
 
-
     // These conditions ignore sanity mode settings, and apply in all rescues, to handle getting stuck in a climb or descend
 
     const float actualAltitudeChange = rescueState.sensor.currentAltitudeCm - prevAltitudeCm;
@@ -540,11 +553,12 @@ static void sensorUpdate(void)
 
     if (rescueState.phase == RESCUE_LANDING) {
         // do this at sensor update rate, not the much slower GPS rate, for quick disarm
-        rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
+        rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z] - acc.dev.acc_1G) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
+        // Note: subtracting 1G from Z assumes the quad is 'flat' with respect to the horizon.  A true non-gravity acceleration value, regardless of attitude, may be better.
     }
 
     rescueState.sensor.directionToHome = GPS_directionToHome;
-    rescueState.sensor.errorAngle = (attitude.values.yaw - rescueState.sensor.directionToHome) * 0.1f;
+    rescueState.sensor.errorAngle = (attitude.values.yaw - rescueState.sensor.directionToHome) / 10.0f;
     // both attitude and direction are in degrees * 10, errorAngle is degrees
     if (rescueState.sensor.errorAngle <= -180) {
         rescueState.sensor.errorAngle += 360;
@@ -562,21 +576,51 @@ static void sensorUpdate(void)
     rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f;
     rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
 
-    static timeUs_t previousGPSDataTimeUs = 0;
-    const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousGPSDataTimeUs);
-    rescueState.sensor.gpsDataIntervalSeconds = constrainf(gpsDataIntervalUs * 0.000001f, 0.01f, 1.0f);
+    rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds();
     // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
-    previousGPSDataTimeUs = currentTimeUs;
 
-    rescueState.sensor.velocityToHomeCmS = (prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds;
+    // when there is a flyaway due to IMU disorientation, increase IMU yaw CoG gain, and reduce max pitch angle
+    if (gpsRescueConfig()->rescueGroundspeed) {
+        const float rescueGroundspeed = 1000.0f; // in cm/s, fixed 10 m/s groundspeed
+        // rescueGroundspeed sets how aggressively the IMU COG Gain increases as velocity error increases
+
+        const float groundspeedErrorRatio = 1.0f + fminf(fabsf(rescueState.sensor.groundSpeedCmS - rescueState.sensor.velocityToHomeCmS) / rescueGroundspeed, 2.0f);
+        // 1 if groundspeed = velocity to home, or both are zero
+        // 2 if forward velocity is zero but sideways speed is 10m/s
+        // 3 if moving backwards at 10m/s.  3 is the maximum allowed value
+
+        // increase IMU COG Gain in proportion to positive pitch angle
+        // pitch angle is positive early in a rescue, and associates with a nose forward ground course
+        float pitchAngleImuGain = (gpsRescueAngle[AI_PITCH] > 0.0f) ? gpsRescueAngle[AI_PITCH] / 2000.0f : 0.0f;
+        // note: gpsRescueAngle[AI_PITCH] is in degrees * 100, and is halved when the IMU is 180 wrong
+        // pitchAngleImuGain is 0 when flat
+        // pitchAngleImuGain is 0.75 if pitch angle is 15 degrees (ie with rescue angle of 30 and 180deg IMU error)
+        // pitchAngleImuGain is 1.5 if pitch angle is 30 degrees (ie with rescue angle of 60 and 180deg IMU error)
+        // pitchAngleImuGain is 3.0 if pitch angle is 60 degrees towards home (unlikely to be sustained at that angle)
+
+        if (rescueState.phase != RESCUE_FLY_HOME) {
+            // prevent IMU disorientation arising from drift during climb, rotate or do nothing phases, which have zero pitch angle
+            // in descent, or too close, increase IMU yaw gain as pitch angle increases
+            rescueState.sensor.imuYawCogGain = pitchAngleImuGain;
+        } else {
+            // during fly home phase also consider the whether the quad is flying towards or away from home
+            // no additional increase in pitch related IMU gain when flying directly towards home
+            // max initial IMU gain with 180 degree disorientation is 5x at 60 deg set, and 3.75x at 30 deg set
+            rescueState.sensor.imuYawCogGain = fminf((0.5f + pitchAngleImuGain) * groundspeedErrorRatio, 5.0f);
+        }
+
+        // cut pitch angle by up to half when groundspeed error is high
+        // minimises flyaway velocity, but reduces ability to handle high wind
+        // groundspeedErrorRatio falls towards zero as the forward speed vector matches the correct path
+        rescueState.sensor.groundspeedPitchAttenuator = 1.0f / fminf(groundspeedErrorRatio, 2.0f);
+    }
+
+    rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds);
     // positive = towards home.  First value is useless since prevDistanceToHomeCm was zero.
     prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
 
-    rescueState.sensor.maxPitchStep = rescueState.sensor.gpsDataIntervalSeconds * GPS_RESCUE_MAX_PITCH_RATE;
-
     DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 2, lrintf(rescueState.sensor.velocityToHomeCmS));
     DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 0, lrintf(rescueState.sensor.velocityToHomeCmS));
-
 }
 
 // This function flashes "RESCUE N/A" in the OSD if:
@@ -641,39 +685,68 @@ void disarmOnImpact(void)
 void descend(void)
 {
     if (newGPSData) {
+        // consider landing area to be a circle half landing height around home, to avoid overshooting home point
         const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (rescueState.intent.targetLandingAltitudeCm / 200.0f);
-        // considers home to be a circle half landing height around home to avoid overshooting home point
         const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
-        rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea;
+     
+        // increase the velocity lowpass filter cutoff for more aggressive responses when descending, especially close to home
+        // 1.5x when starting descent, 2.5x (smoother) when almost landed
+        rescueState.intent.velocityPidCutoffModifier = 2.5f - proximityToLandingArea;
+
         // reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting.
-        // if quad drifts further than 2m away from home, should by then have rotated towards home, so pitch is allowed
-        rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->angle * proximityToLandingArea;
-        // reduce roll capability when closer to home, none within final 2m
-    }
+        // if it does overshoot, allow pitch angle limit to build up to correct the overshoot once rotated
+        rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea;
+
+        // attenuate velocity iterm towards zero as we get closer to the landing area
+        rescueState.intent.velocityItermAttenuator = proximityToLandingArea;
 
-    // adjust altitude step for interval between altitude readings
-    rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
+        // reduce pitch angle limit if there is a significant groundspeed error - eg on overshooting home
+        rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator;
 
-    // descend more slowly if return altitude is less than 20m
-    const float descentAttenuator = rescueState.intent.returnAltitudeCm / 2000.0f;
-    if (descentAttenuator < 1.0f) {
-        rescueState.intent.altitudeStep *= descentAttenuator;
+        // limit roll angle to half the allowed pitch angle and attenuate when closer to home
+        // keep some roll when at the landing circle distance to avoid endless circling
+        const float proximityToHome = constrainf(rescueState.sensor.distanceToHomeM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
+        rescueState.intent.rollAngleLimitDeg = 0.5f * rescueState.intent.pitchAngleLimitDeg * proximityToHome;
     }
-    // descend more quickly from higher altitude
-    rescueState.intent.descentRateModifier = constrainf(rescueState.intent.targetAltitudeCm / 5000.0f, 0.0f, 1.0f);
-    rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep * (1.0f + (2.0f * rescueState.intent.descentRateModifier));
-    // increase descent rate to max of 3x default above 50m, 2x above 25m, 1.2 at 5m, default by ground level.
+
+    // ensure we have full yaw authority in case we entered descent mode without enough time in fly home to acquire it gracefully
+    rescueState.intent.yawAttenuator = 1.0f;
+
+    // set the altitude step, considering the interval between altitude readings and the descent rate
+    float altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
+
+    // descend more slowly if the return home altitude was less than 20m
+    const float descentRateAttenuator = constrainf (rescueState.intent.returnAltitudeCm / 2000.0f, 0.25f, 1.0f);
+    altitudeStep *= descentRateAttenuator;
+    // slowest descent rate will be 1/4 of normal when we start descending at or below 5m above take-off point.
+    // otherwise a rescue initiated very low and close may not get all the way home
+
+    // descend faster while the quad is at higher altitudes
+    const float descentRateMultiplier = constrainf(rescueState.intent.targetAltitudeCm / 5000.0f, 0.0f, 1.0f);
+    altitudeStep *= 1.0f + (2.0f * descentRateMultiplier); 
+    // maximum descent rate increase is 3x default above 50m, 2x above 25m, 1.2x at 5m, default by ground level
+
+    // also increase throttle D up to 2x in the descent phase when altitude descent rate is faster, for better control
+    rescueState.intent.throttleDMultiplier = 1.0f + descentRateMultiplier;
+
+    rescueState.intent.targetAltitudeCm -= altitudeStep;
 }
 
-void altitudeAchieved(void)
+void initialiseRescueValues (void)
 {
-    rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
-    rescueState.intent.altitudeStep = 0;
-    rescueState.phase = RESCUE_ROTATE;
+    rescueState.intent.secondsFailing = 0; // reset the sanity check timer
+    rescueState.intent.yawAttenuator = 0.0f; // no yaw in the climb
+    rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS; // avoid snap from D at the start
+    rescueState.intent.rollAngleLimitDeg = 0.0f; // no roll until flying home
+    rescueState.intent.throttleDMultiplier = 1.0f;
+    rescueState.intent.velocityPidCutoffModifier = 1.0f; // normal velocity lowpass filter cutoff
+    rescueState.intent.pitchAngleLimitDeg = 0.0f; // force pitch adjustment to zero - level mode will level out
+    rescueState.intent.velocityItermAttenuator = 0.0f; // multiply velocity iTerm by zero
+    rescueState.intent.velocityItermRelax = 0.0f; // don't accumulate any
 }
 
 void gpsRescueUpdate(void)
-// this runs a lot faster than the GPS Data update rate, and runs whether or not rescue is active
+// runs at gpsRescueTaskIntervalSeconds, and runs whether or not rescue is active
 {
     if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
         rescueStop(); // sets phase to RESCUE_IDLE; does nothing else.  RESCUE_IDLE tasks still run.
@@ -687,7 +760,8 @@ void gpsRescueUpdate(void)
 
     sensorUpdate(); // always get latest GPS and Altitude data, update ascend and descend rates
 
-    bool startedLow = true;
+    static bool initialAltitudeLow = true;
+    static bool initialVelocityLow = true;
     rescueState.isAvailable = checkGPSRescueIsAvailable();
 
     switch (rescueState.phase) {
@@ -700,32 +774,30 @@ void gpsRescueUpdate(void)
         // target altitude is always set to current altitude.
 
     case RESCUE_INITIALIZE:
-        // Things that should abort the start of a Rescue
+        // Things that should be done at the start of a Rescue
+        rescueState.intent.targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
         if (!STATE(GPS_FIX_HOME)) {
             // we didn't get a home point on arming
             rescueState.failure = RESCUE_NO_HOME_POINT;
-            // will result in a disarm via the sanity check system, with delay if switch induced
-            // alternative is to prevent the rescue by returning to IDLE, but this could cause flyaways
-        } else if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
-            // Attempt to initiate inside minimum activation distance -> landing mode
-            rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
-            rescueState.intent.targetVelocityCmS = 0; // zero forward velocity
-            rescueState.intent.pitchAngleLimitDeg = 0; // flat on pitch
-            rescueState.intent.rollAngleLimitDeg = 0.0f; // flat on roll also
-            rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueState.intent.altitudeStep;
-            rescueState.phase = RESCUE_LANDING;
-            // start landing from current altitude
+            // will result in a disarm via the sanity check system, or float around if switch induced
         } else {
-            rescueState.phase = RESCUE_ATTAIN_ALT;
-            rescueState.intent.secondsFailing = 0; // reset the sanity check timer for the climb
-            rescueState.intent.targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
-            startedLow = (rescueState.sensor.currentAltitudeCm <= rescueState.intent.returnAltitudeCm);
-            rescueState.intent.yawAttenuator = 0.0f;
-            rescueState.intent.targetVelocityCmS = 0.0f; // zero forward velocity while climbing
-            rescueState.intent.pitchAngleLimitDeg = 0.0f; // no pitch
-            rescueState.intent.rollAngleLimitDeg = 0.0f; // no roll until flying home
-            rescueState.intent.altitudeStep = 0.0f;
-            rescueState.intent.descentRateModifier = 0.0f;
+            if (rescueState.sensor.distanceToHomeM < 5.0f && rescueState.sensor.currentAltitudeCm < rescueState.intent.targetLandingAltitudeCm) {
+                // attempted initiation within 5m of home, and 'on the ground' -> instant disarm, for safety reasons
+                rescueState.phase = RESCUE_ABORT;
+            } else {
+                // attempted initiation within minimum rescue distance requires us to fly out to at least that distance
+                if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
+                    // climb above current height by buffer height, to at least 10m altitude
+                    rescueState.intent.returnAltitudeCm = fmaxf(1000.0f, rescueState.sensor.currentAltitudeCm + (gpsRescueConfig()->rescueAltitudeBufferM * 100.0f));
+                    // note that the pitch controller will pitch forward to fly out to minRescueDth
+                    // set the descent distance to half the minimum rescue distance. which we should have moved out to in the climb phase
+                    rescueState.intent.descentDistanceM = 0.5f * gpsRescueConfig()->minRescueDth;
+                }
+                // otherwise behave as for a normal rescue
+                initialiseRescueValues ();
+                initialAltitudeLow = (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm);
+                rescueState.phase = RESCUE_ATTAIN_ALT;
+            }
         }
         break;
 
@@ -733,48 +805,65 @@ void gpsRescueUpdate(void)
         // gradually increment the target altitude until the craft reaches target altitude
         // note that this can mean the target altitude may increase above returnAltitude if the craft lags target
         // sanity check will abort if altitude gain is blocked for a cumulative period
-        if (startedLow) {
-            if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) {
-                rescueState.intent.altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->ascendRate;
-            } else if (rescueState.sensor.currentAltitudeCm > rescueState.intent.returnAltitudeCm) {
-                altitudeAchieved();
-            } 
+        if (initialAltitudeLow == (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm)) {
+            // we started low, and still are low; also true if we started high, and still are too high
+            const float altitudeStep = ((initialAltitudeLow) ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * rescueState.sensor.gpsRescueTaskIntervalSeconds;
+            rescueState.intent.targetAltitudeCm += altitudeStep;
         } else {
-            if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) {
-                rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
-            } else if (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm) {
-                altitudeAchieved();
+            // target altitude achieved - move on to ROTATE phase, returning at target altitude
+            rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
+            // if initiated too close, do not rotate or do anything else until sufficiently far away that a 'normal' rescue can happen
+            if (rescueState.sensor.distanceToHomeM > gpsRescueConfig()->minRescueDth) {
+                rescueState.phase = RESCUE_ROTATE;
             }
         }
-        rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
+
+        // give velocity P and I no error that otherwise could be present due to velocity drift at the start of the rescue
+        rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS;
         break;
 
     case RESCUE_ROTATE:
-        if (rescueState.intent.yawAttenuator < 1.0f) { // gradually acquire yaw authority
-            rescueState.intent.yawAttenuator += 0.01f;
+        if (rescueState.intent.yawAttenuator < 1.0f) { // acquire yaw authority over one second
+            rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
         }
         if (rescueState.sensor.absErrorAngle < 30.0f) {
-            rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->angle; // allow pitch
+            // allow pitch, limiting allowed angle if we are drifting away from home
+            rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator;
             rescueState.phase = RESCUE_FLY_HOME; // enter fly home phase
             rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home
+            rescueState.intent.velocityItermRelax = 1.0f; // velocity iTerm activated
         }
+        initialVelocityLow = rescueState.sensor.velocityToHomeCmS < gpsRescueConfig()->rescueGroundspeed; // used to set direction of velocity target change
+        rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS;
         break;
 
     case RESCUE_FLY_HOME:
         if (rescueState.intent.yawAttenuator < 1.0f) { // be sure to accumulate full yaw authority
-            rescueState.intent.yawAttenuator += 0.01f;
+            rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
         }
-        // steadily increase target velocity target until full return velocity is acquired
-        if (rescueState.intent.targetVelocityCmS < gpsRescueConfig()->rescueGroundspeed) {
-            rescueState.intent.targetVelocityCmS += 0.01f * gpsRescueConfig()->rescueGroundspeed;
+        // velocity PIDs are now active
+        // update target velocity gradually, aiming for rescueGroundspeed with a time constant of 1.0s
+        const float targetVelocityError = gpsRescueConfig()->rescueGroundspeed - rescueState.intent.targetVelocityCmS;
+        const float velocityTargetStep = rescueState.sensor.gpsRescueTaskIntervalSeconds * targetVelocityError;
+        // velocityTargetStep is positive when starting low, negative when starting high
+        const bool targetVelocityIsLow = rescueState.intent.targetVelocityCmS < gpsRescueConfig()->rescueGroundspeed;
+        if (initialVelocityLow == targetVelocityIsLow) {
+            // also true if started faster than target velocity and target is still high
+            rescueState.intent.targetVelocityCmS += velocityTargetStep;
         }
-        // acquire full roll authority slowly when pointing to home
-        if (rescueState.sensor.absErrorAngle < 10.0f && rescueState.intent.rollAngleLimitDeg < gpsRescueConfig()->angle) {
-            // roll is primarily intended to deal with wind drift causing small yaw errors during return
-            rescueState.intent.rollAngleLimitDeg += 0.1f;
-        } 
+
+        // slowly introduce velocity iTerm accumulation at start, goes 0 ->1 with time constant 2.0s
+        rescueState.intent.velocityItermRelax += 0.5f * rescueState.sensor.gpsRescueTaskIntervalSeconds * (1.0f - rescueState.intent.velocityItermRelax);
+        // there is always a lot of lag at the start, this gradual start avoids excess iTerm accumulation
+
+        rescueState.intent.velocityPidCutoffModifier = 2.0f - rescueState.intent.velocityItermRelax; 
+        // higher velocity filter cutoff for initial few seconds to improve accuracy; can be smoother later
 
         if (newGPSData) {
+            // cut back on allowed angle if there is a high groundspeed error
+            rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.sensor.groundspeedPitchAttenuator;
+            // introduce roll slowly and limit to half the max pitch angle; earth referenced yaw may add more roll via angle code
+            rescueState.intent.rollAngleLimitDeg = 0.5f * rescueState.intent.pitchAngleLimitDeg * rescueState.intent.velocityItermRelax; 
             if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) {
                 rescueState.phase = RESCUE_DESCENT;
                 rescueState.intent.secondsFailing = 0; // reset sanity timer for descent
@@ -795,6 +884,7 @@ void gpsRescueUpdate(void)
     case RESCUE_LANDING:
         // Reduce altitude target steadily until impact, then disarm.
         // control yaw angle and throttle and pitch, attenuate velocity, roll and pitch iTerm
+        // increase velocity smoothing cutoff as we get closer to ground
         descend();
         disarmOnImpact();
         break;
@@ -806,6 +896,7 @@ void gpsRescueUpdate(void)
     case RESCUE_ABORT:
         setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
         disarm(DISARM_REASON_FAILSAFE);
+        rescueState.intent.secondsFailing = 0; // reset sanity timers so we can re-arm
         rescueStop();
         break;
 
@@ -832,6 +923,11 @@ float gpsRescueGetYawRate(void)
     return rescueYaw;
 }
 
+float gpsRescueGetImuYawCogGain(void)
+{
+    return rescueState.sensor.imuYawCogGain;
+}
+
 float gpsRescueGetThrottle(void)
 {
     // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.
diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h
index 7e77e7121c8..9fb2897dfc4 100644
--- a/src/main/flight/gps_rescue.h
+++ b/src/main/flight/gps_rescue.h
@@ -57,3 +57,4 @@ bool gpsRescueIsConfigured(void);
 bool gpsRescueIsAvailable(void);
 bool gpsRescueIsDisabled(void);
 bool gpsRescueDisableMag(void);
+float gpsRescueGetImuYawCogGain(void);
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 613c801cbf8..a308cfd3348 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -201,7 +201,7 @@ static float invSqrt(float x)
 static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                 bool useAcc, float ax, float ay, float az,
                                 bool useMag,
-                                bool useCOG, float courseOverGround, const float dcmKpGain)
+                                float cogYawGain, float courseOverGround, const float dcmKpGain)
 {
     static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
 
@@ -210,17 +210,15 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 
     // Use raw heading error (from GPS or whatever else)
     float ex = 0, ey = 0, ez = 0;
-    if (useCOG) {
+    if (cogYawGain != 0.0f) {
+        // Used in a GPS Rescue to boost IMU yaw gain when course over ground and velocity to home differ significantly
         while (courseOverGround >  M_PIf) {
             courseOverGround -= (2.0f * M_PIf);
         }
-
         while (courseOverGround < -M_PIf) {
             courseOverGround += (2.0f * M_PIf);
         }
-
-        const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
-
+        const float ez_ef = cogYawGain * (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
         ex = rMat[2][0] * ez_ef;
         ey = rMat[2][1] * ez_ef;
         ez = rMat[2][2] * ez_ef;
@@ -263,7 +261,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     // Use measured acceleration vector
     float recipAccNorm = sq(ax) + sq(ay) + sq(az);
     if (useAcc && recipAccNorm > 0.01f) {
-        // Normalise accelerometer measurement
+        // Normalise accelerometer measurement; useAcc is true when all smoothed acc axes are within 20% of 1G
         recipAccNorm = invSqrt(recipAccNorm);
         ax *= recipAccNorm;
         ay *= recipAccNorm;
@@ -415,10 +413,10 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera
     if (attitudeResetActive) {
         ret = ATTITUDE_RESET_KP_GAIN;
     } else {
-       ret = imuRuntimeConfig.dcm_kp;
-       if (!armState) {
-          ret = ret * 10.0f; // Scale the kP to generally converge faster when disarmed.
-       }
+        ret = imuRuntimeConfig.dcm_kp;
+        if (!armState) {
+            ret *= 10.0f; // Scale the kP to generally converge faster when disarmed.
+        }
     }
 
     return ret;
@@ -476,8 +474,8 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     static timeUs_t previousIMUUpdateTime;
     bool useAcc = false;
     bool useMag = false;
-    bool useCOG = false; // Whether or not correct yaw via imuMahonyAHRSupdate from our ground course
-    float courseOverGround = 0; // To be used when useCOG is true.  Stored in Radians
+    float cogYawGain = 0.0f; // IMU yaw gain to be applied in imuMahonyAHRSupdate from ground course, default to no correction from CoG
+    float courseOverGround = 0; // To be used when cogYawGain is non-zero, in radians
 
     const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
     previousIMUUpdateTime = currentTimeUs;
@@ -495,14 +493,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
         // Use GPS course over ground to correct attitude.values.yaw
         courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-        useCOG = true;
-
+        cogYawGain = (FLIGHT_MODE(GPS_RESCUE_MODE)) ? gpsRescueGetImuYawCogGain() : 1.0f;
+        // normally update yaw heading with GPS data, but when in a Rescue, modify the IMU yaw gain dynamically
         if (shouldInitializeGPSHeading()) {
-            // Reset our reference and reinitialize quaternion.  This will likely ideally happen more than once per flight, but for now,
+            // Reset our reference and reinitialize quaternion.
             // shouldInitializeGPSHeading() returns true only once.
             imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
-
-            useCOG = false; // Don't use the COG when we first reinitialize.  Next time around though, yes.
+            cogYawGain = 0.0f; // Don't use the COG when we first initialize
         }
     }
 #endif
@@ -512,7 +509,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     UNUSED(imuIsAccelerometerHealthy);
     UNUSED(useAcc);
     UNUSED(useMag);
-    UNUSED(useCOG);
+    UNUSED(cogYawGain);
     UNUSED(canUseGPSHeading);
     UNUSED(courseOverGround);
     UNUSED(deltaT);
@@ -528,13 +525,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
         gyroAverage[axis] = gyroGetFilteredDownsampled(axis);
     }
 
-    useAcc = imuIsAccelerometerHealthy(acc.accADC);
+    useAcc = imuIsAccelerometerHealthy(acc.accADC); // all smoothed accADC values are within 20% of 1G
 
     imuMahonyAHRSupdate(deltaT * 1e-6f,
                         DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
                         useAcc, acc.accADC[X], acc.accADC[Y], acc.accADC[Z],
                         useMag,
-                        useCOG, courseOverGround,  imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
+                        cogYawGain, courseOverGround,  imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
 
     imuUpdateEulerAngles();
 #endif
diff --git a/src/main/flight/position.c b/src/main/flight/position.c
index a47a26617a2..5c476fd7f75 100644
--- a/src/main/flight/position.c
+++ b/src/main/flight/position.c
@@ -198,7 +198,6 @@ void calculateEstimatedAltitude(void)
 #ifdef USE_VARIO
     estimatedVario = lrintf(zeroedAltitudeDerivative);
     estimatedVario = applyDeadband(estimatedVario, 10); // ignore climb rates less than 0.1 m/s
-    estimatedVario = constrain(estimatedVario, -1500, 1500);
 #endif
  
     // *** set debugs
diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c
index 83d03152365..d6a60ae9ee5 100644
--- a/src/main/io/beeper.c
+++ b/src/main/io/beeper.c
@@ -400,19 +400,22 @@ void beeperUpdate(timeUs_t currentTimeUs)
         return;
     }
 
-    if (!beeperIsOn) {
 #ifdef USE_DSHOT
-        if (!areMotorsRunning()
-            && ((currentBeeperEntry->mode == BEEPER_RX_SET && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_SET)))
-            || (currentBeeperEntry->mode == BEEPER_RX_LOST && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_LOST))))) {
-
-            if ((currentTimeUs - getLastDisarmTimeUs() > DSHOT_BEACON_GUARD_DELAY_US) && !isTryingToArm()) {
+    if (!areMotorsRunning() && (currentBeeperEntry->mode == BEEPER_RX_SET || currentBeeperEntry->mode == BEEPER_RX_LOST)
+        && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(currentBeeperEntry->mode))) {
+        if (cmpTimeUs(currentTimeUs, getLastDisarmTimeUs()) > DSHOT_BEACON_GUARD_DELAY_US && !isTryingToArm()) {
+            const timeDelta_t dShotBeaconInterval = (currentBeeperEntry->mode == BEEPER_RX_SET) ? DSHOT_BEACON_MODE_INTERVAL_US : DSHOT_BEACON_RXLOSS_INTERVAL_US;
+            if (cmpTimeUs(currentTimeUs, lastDshotBeaconCommandTimeUs) > dShotBeaconInterval) {
+                // at least 500ms between DShot beacons to allow time for the sound to fully complete
+                // the DShot Beacon tone duration is determined by the ESC, and should not exceed 250ms
                 lastDshotBeaconCommandTimeUs = currentTimeUs;
                 dshotCommandWrite(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, DSHOT_CMD_TYPE_INLINE);
             }
         }
+    }
 #endif
 
+    if (!beeperIsOn) {
         if (currentBeeperEntry->sequence[beeperPos] != 0) {
             if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(currentBeeperEntry->mode))) {
                 BEEP_ON;
diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h
index 2b177964e5b..af71926f075 100644
--- a/src/main/io/beeper.h
+++ b/src/main/io/beeper.h
@@ -27,6 +27,9 @@
 #ifdef USE_DSHOT
 #define DSHOT_BEACON_GUARD_DELAY_US 1200000  // Time to separate dshot beacon and armining/disarming events
                                              // to prevent interference with motor direction commands
+#define DSHOT_BEACON_MODE_INTERVAL_US     450000  // at least 450ms between successive DShot beacon iterations to allow time for ESC to play tone
+#define DSHOT_BEACON_RXLOSS_INTERVAL_US   950000  // at least 950ms between successive DShot beacon iterations to allow time for ESC to play tone
+                                                  // we check beeper every 100ms, so these result in 500ms and 1.0s in practice
 #endif
 
 typedef enum {
diff --git a/src/main/io/displayport_crsf.c b/src/main/io/displayport_crsf.c
index cd056ce0fae..68a49ee5c2c 100644
--- a/src/main/io/displayport_crsf.c
+++ b/src/main/io/displayport_crsf.c
@@ -96,7 +96,7 @@ static int crsfWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row,
 
 static int crsfWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t attr, uint8_t c)
 {
-    char s[1];
+    char s[2];
     tfp_sprintf(s, "%c", c);
     return crsfWriteString(displayPort, col, row, attr, s);
 }
diff --git a/src/main/io/displayport_hott.c b/src/main/io/displayport_hott.c
index 1dec6bab016..03161b8188d 100644
--- a/src/main/io/displayport_hott.c
+++ b/src/main/io/displayport_hott.c
@@ -56,7 +56,7 @@ static int hottWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row,
     UNUSED(attr);
 
     while (*s) {
-        hottWriteChar(displayPort,  col++, row, DISPLAYPORT_ATTR_NORMAL, *(s++));
+        hottWriteChar(displayPort,  col++, row, DISPLAYPORT_SEVERITY_NORMAL, *(s++));
     }
     return 0;
 }
@@ -67,7 +67,7 @@ static int hottClearScreen(displayPort_t *displayPort, displayClearOption_e opti
 
     for (int row = 0; row < displayPort->rows; row++) {
         for (int col= 0; col < displayPort->cols; col++) {
-            hottWriteChar(displayPort, col, row, DISPLAYPORT_ATTR_NORMAL, ' ');
+            hottWriteChar(displayPort, col, row, DISPLAYPORT_SEVERITY_NORMAL, ' ');
         }
     }
     return 0;
diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c
index 4f0ccebb06f..52dff222f4c 100644
--- a/src/main/io/displayport_msp.c
+++ b/src/main/io/displayport_msp.c
@@ -119,9 +119,9 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, uin
     buf[0] = MSP_DP_WRITE_STRING;
     buf[1] = row;
     buf[2] = col;
-    buf[3] = displayPortProfileMsp()->fontSelection[attr] & ~DISPLAYPORT_MSP_ATTR_BLINK & DISPLAYPORT_MSP_ATTR_MASK;
+    buf[3] = displayPortProfileMsp()->fontSelection[attr & (DISPLAYPORT_SEVERITY_COUNT - 1)] & DISPLAYPORT_MSP_ATTR_FONT;
 
-    if (attr & DISPLAYPORT_ATTR_BLINK) {
+    if (attr & DISPLAYPORT_BLINK) {
         buf[3] |= DISPLAYPORT_MSP_ATTR_BLINK;
     }
 
diff --git a/src/main/io/displayport_msp.h b/src/main/io/displayport_msp.h
index e60184382a6..2d89616ad83 100644
--- a/src/main/io/displayport_msp.h
+++ b/src/main/io/displayport_msp.h
@@ -42,7 +42,7 @@ typedef enum {
 #define DISPLAYPORT_MSP_ATTR_VERSION BIT(7) // Format indicator; must be zero for V2 (and V1)
 #define DISPLAYPORT_MSP_ATTR_BLINK   BIT(6) // Device local blink
 #define DISPLAYPORT_MSP_ATTR_FONT    (BIT(0) | BIT(1)) // Select bank of 256 characters as per displayPortSeverity_e
-#define DISPLAYPORT_MSP_ATTR_MASK    (~(DISPLAYPORT_MSP_ATTR_VERSION | DISPLAYPORT_MSP_ATTR_BLINK | DISPLAYPORT_MSP_ATTR_FONT))
+#define DISPLAYPORT_MSP_ATTR_MASK    (DISPLAYPORT_MSP_ATTR_VERSION | DISPLAYPORT_MSP_ATTR_BLINK | DISPLAYPORT_MSP_ATTR_FONT)
 
 struct displayPort_s *displayPortMspInit(void);
 void displayPortMspSetSerial(serialPortIdentifier_e serialPort);
diff --git a/src/main/io/displayport_srxl.c b/src/main/io/displayport_srxl.c
index 2e9d2e946ec..d12767d479e 100644
--- a/src/main/io/displayport_srxl.c
+++ b/src/main/io/displayport_srxl.c
@@ -71,15 +71,15 @@ static int srxlClearScreen(displayPort_t *displayPort, displayClearOption_e opti
     UNUSED(options);
     for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) {
         for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) {
-            srxlWriteChar(displayPort, col, row, DISPLAYPORT_ATTR_NORMAL, ' ');
+            srxlWriteChar(displayPort, col, row, DISPLAYPORT_SEVERITY_NORMAL, ' ');
         }
     }
-    srxlWriteString(displayPort, 1, 0, DISPLAYPORT_ATTR_NORMAL, "BETAFLIGHT");
+    srxlWriteString(displayPort, 1, 0, DISPLAYPORT_SEVERITY_NORMAL, "BETAFLIGHT");
 
     if (displayPort->grabCount == 0) {
-        srxlWriteString(displayPort, 0, 2, DISPLAYPORT_ATTR_NORMAL, CMS_STARTUP_HELP_TEXT1);
-        srxlWriteString(displayPort, 2, 3, DISPLAYPORT_ATTR_NORMAL, CMS_STARTUP_HELP_TEXT2);
-        srxlWriteString(displayPort, 2, 4, DISPLAYPORT_ATTR_NORMAL, CMS_STARTUP_HELP_TEXT3);
+        srxlWriteString(displayPort, 0, 2, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT1);
+        srxlWriteString(displayPort, 2, 3, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT2);
+        srxlWriteString(displayPort, 2, 4, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT3);
     }
     return 0;
 }
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 7d548c98f89..a399898e265 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -82,7 +82,6 @@ uint16_t GPS_distanceToHome;        // distance to home point in meters
 uint32_t GPS_distanceToHomeCm;
 int16_t GPS_directionToHome;        // direction to home or hol point in degrees * 10
 uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
-int16_t GPS_verticalSpeedInCmS;     // vertical speed in cm/s
 int16_t nav_takeoff_bearing;
 
 #define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 0.54 km/h 0.335 mph
@@ -107,7 +106,7 @@ uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];
 #define UBLOX_ACK_TIMEOUT_MAX_COUNT (25)
 
 static serialPort_t *gpsPort;
-static float gpsSampleRateHz;
+static float gpsDataIntervalSeconds;
 
 typedef struct gpsInitData_s {
     uint8_t index;
@@ -308,8 +307,7 @@ static void gpsSetState(gpsState_e state)
 
 void gpsInit(void)
 {
-    gpsSampleRateHz = 0.0f;
-
+    gpsDataIntervalSeconds = 0.1f;
     gpsData.baudrateIndex = 0;
     gpsData.errors = 0;
     gpsData.timeouts = 0;
@@ -340,14 +338,20 @@ void gpsInit(void)
     }
 
     portMode_e mode = MODE_RXTX;
+    portOptions_e options = SERIAL_NOT_INVERTED;
+
 #if defined(GPS_NMEA_TX_ONLY)
     if (gpsConfig()->provider == GPS_NMEA) {
         mode &= ~MODE_TX;
     }
 #endif
 
+    if ((gpsPortConfig->identifier >= SERIAL_PORT_USART1) && (gpsPortConfig->identifier <= SERIAL_PORT_USART10)){
+        options |= SERIAL_CHECK_TX;
+    }
+
     // no callback - buffer will be consumed in gpsUpdate()
-    gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED);
+    gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, options);
     if (!gpsPort) {
         return;
     }
@@ -784,10 +788,27 @@ void gpsUpdate(timeUs_t currentTimeUs)
         }
         // Restore default task rate
         rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE));
-   } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
-        gpsSetState(GPS_STATE_RECEIVING_DATA);
-        onGpsNewData();
-        GPS_update &= ~GPS_MSP_UPDATE;
+    } else if (gpsConfig()->provider == GPS_MSP) {
+        if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
+            if (gpsData.state == GPS_STATE_INITIALIZED) {
+                gpsSetState(GPS_STATE_RECEIVING_DATA);
+            }
+
+            // Data is available
+            gpsData.lastMessage = millis();
+            sensorsSet(SENSOR_GPS);
+
+            GPS_update ^= GPS_DIRECT_TICK;
+
+            onGpsNewData();
+
+            GPS_update &= ~GPS_MSP_UPDATE;
+        } else {
+            // check for no data/gps timeout/cable disconnection etc
+            if (cmp32(millis(), gpsData.lastMessage) > GPS_TIMEOUT) {
+                gpsSetState(GPS_STATE_LOST_COMMUNICATION);
+            }
+        }
     }
 
 #if DEBUG_UBLOX_INIT
@@ -825,37 +846,39 @@ void gpsUpdate(timeUs_t currentTimeUs)
                 gpsSetState(GPS_STATE_LOST_COMMUNICATION);
 #ifdef USE_GPS_UBLOX
             } else {
-                if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled
-                    switch (gpsData.state_position) {
-                        case 0:
-                            if (!isConfiguratorConnected()) {
-                                if (gpsData.ubloxUseSAT) {
-                                    ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 0); // disable SAT MSG
-                                } else {
-                                    ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 0); // disable SVINFO MSG
+                if (gpsConfig()->provider != GPS_MSP) {
+                    if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled
+                        switch (gpsData.state_position) {
+                            case 0:
+                                if (!isConfiguratorConnected()) {
+                                    if (gpsData.ubloxUseSAT) {
+                                        ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 0); // disable SAT MSG
+                                    } else {
+                                        ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 0); // disable SVINFO MSG
+                                    }
+                                    gpsData.state_position = 1;
+                                }
+                                break;
+                            case 1:
+                                if (STATE(GPS_FIX) && (gpsConfig()->gps_ublox_mode == UBLOX_DYNAMIC)) {
+                                    ubloxSendNAV5Message(true);
+                                    gpsData.state_position = 2;
                                 }
-                                gpsData.state_position = 1;
-                            }
-                            break;
-                        case 1:
-                            if (STATE(GPS_FIX) && (gpsConfig()->gps_ublox_mode == UBLOX_DYNAMIC)) {
-                                ubloxSendNAV5Message(true);
-                                gpsData.state_position = 2;
-                            }
-                            if (isConfiguratorConnected()) {
-                                gpsData.state_position = 2;
-                            }
-                            break;
-                        case 2:
-                            if (isConfiguratorConnected()) {
-                                if (gpsData.ubloxUseSAT) {
-                                    ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles)
-                                } else {
-                                    ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles)
+                                if (isConfiguratorConnected()) {
+                                    gpsData.state_position = 2;
                                 }
-                                gpsData.state_position = 0;
-                            }
-                            break;
+                                break;
+                            case 2:
+                                if (isConfiguratorConnected()) {
+                                    if (gpsData.ubloxUseSAT) {
+                                        ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5); // set SAT MSG rate (every 5 cycles)
+                                    } else {
+                                        ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5); // set SVINFO MSG rate (every 5 cycles)
+                                    }
+                                    gpsData.state_position = 0;
+                                }
+                                break;
+                        }
                     }
                 }
 #endif //USE_GPS_UBLOX
@@ -1449,12 +1472,12 @@ static uint8_t _ck_b;
 
 // State machine state
 static bool _skip_packet;
-static uint8_t _step;
+static uint8_t _step = 0;
 static uint8_t _msg_id;
 static uint16_t _payload_length;
 static uint16_t _payload_counter;
 
-static bool next_fix;
+static bool next_fix = false;
 static uint8_t _class;
 
 // do we have new position information?
@@ -1824,19 +1847,15 @@ void GPS_calc_longitude_scaling(int32_t lat)
 }
 
 ////////////////////////////////////////////////////////////////////////////////////
-// Calculate the distance flown and vertical speed from gps position data
+// Calculate the distance flown from gps position data
 //
-static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
+static void GPS_calculateDistanceFlown(bool initialize)
 {
     static int32_t lastCoord[2] = { 0, 0 };
     static int32_t lastAlt;
-    static int32_t lastMillis;
-
-    int currentMillis = millis();
 
     if (initialize) {
         GPS_distanceFlownInCm = 0;
-        GPS_verticalSpeedInCmS = 0;
     } else {
         if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
             uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed;
@@ -1851,13 +1870,10 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
                 GPS_distanceFlownInCm += dist;
             }
         }
-        GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
-        GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
     }
     lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
     lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
     lastAlt = gpsSol.llh.altCm;
-    lastMillis = currentMillis;
 }
 
 void GPS_reset_home_position(void)
@@ -1876,7 +1892,7 @@ void GPS_reset_home_position(void)
             // PS: to test for gyro cal, check for !ARMED, since we cannot be here while disarmed other than via gyro cal
         }
     }
-    GPS_calculateDistanceFlownVerticalSpeed(true); // Initialize
+    GPS_calculateDistanceFlown(true); // Initialize
 }
 
 ////////////////////////////////////////////////////////////////////////////////////
@@ -1901,8 +1917,8 @@ void GPS_calculateDistanceAndDirectionToHome(void)
         uint32_t dist;
         int32_t dir;
         GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
-        GPS_distanceToHome = dist / 100; // m/s
-        GPS_distanceToHomeCm = dist; // cm/sec
+        GPS_distanceToHome = dist / 100; // m
+        GPS_distanceToHomeCm = dist; // cm
         GPS_directionToHome = dir / 10; // degrees * 10 or decidegrees
     } else {
         // If we don't have home set, do not display anything
@@ -1914,21 +1930,31 @@ void GPS_calculateDistanceAndDirectionToHome(void)
 
 void onGpsNewData(void)
 {
-    static timeUs_t timeUs, lastTimeUs = 0;
+    static timeUs_t lastTimeUs = 0;
+    const timeUs_t timeUs = micros();
+
+    // calculate GPS solution interval
+    // !!! TOO MUCH JITTER TO BE USEFUL - need an exact time !!!
+    const float gpsDataIntervalS = cmpTimeUs(timeUs, lastTimeUs) / 1e6f;
+    // dirty hack to remove jitter from interval
+    if (gpsDataIntervalS < 0.15f) {
+        gpsDataIntervalSeconds = 0.1f;
+    } else if (gpsDataIntervalS < 0.4f) {
+        gpsDataIntervalSeconds = 0.2f;
+    } else {
+        gpsDataIntervalSeconds = 1.0f;
+    }
 
-    // Detect current sample rate of GPS solution
-    timeUs = micros();
-    gpsSampleRateHz = 1e6f / cmpTimeUs(timeUs, lastTimeUs);
     lastTimeUs = timeUs;
 
-    if (!(STATE(GPS_FIX) && gpsSol.numSat >= GPS_MIN_SAT_COUNT)) {
+    if (!STATE(GPS_FIX)) {
         // if we don't have a 3D fix and the minimum sats, don't give data to GPS rescue
         return;
     }
 
     GPS_calculateDistanceAndDirectionToHome();
     if (ARMING_FLAG(ARMED)) {
-        GPS_calculateDistanceFlownVerticalSpeed(false);
+        GPS_calculateDistanceFlown(false);
     }
 
 #ifdef USE_GPS_RESCUE
@@ -1946,9 +1972,9 @@ void gpsSetFixState(bool state)
     }
 }
 
-float gpsGetSampleRateHz(void)
+float getGpsDataIntervalSeconds(void)
 {
-    return gpsSampleRateHz;
+    return gpsDataIntervalSeconds;
 }
 
 #endif // USE_GPS
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index ae609a931e5..ecb1460a594 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -150,7 +150,6 @@ extern uint16_t GPS_distanceToHome;        // distance to home point in meters
 extern uint32_t GPS_distanceToHomeCm;      // distance to home point in cm
 extern int16_t GPS_directionToHome;        // direction to home or hol point in degrees
 extern uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
-extern int16_t GPS_verticalSpeedInCmS;     // vertical speed in cm/s
 extern int16_t GPS_angle[ANGLE_INDEX_COUNT];                // it's the angles that must be applied for GPS correction
 extern float GPS_scaleLonDown;  // this is used to offset the shrinking longitude as we go towards the poles
 extern int16_t nav_takeoff_bearing;
@@ -214,4 +213,4 @@ void GPS_reset_home_position(void);
 void GPS_calc_longitude_scaling(int32_t lat);
 void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);
 void gpsSetFixState(bool state);
-float gpsGetSampleRateHz(void);
+float getGpsDataIntervalSeconds(void);
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 7662e98a3cb..5e78f1c300e 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -938,6 +938,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
         break;
     }
 
+#if defined(USE_OSD)
     case MSP_OSD_CONFIG: {
 #define OSD_FLAGS_OSD_FEATURE           (1 << 0)
 //#define OSD_FLAGS_OSD_SLAVE             (1 << 1)
@@ -948,7 +949,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
 #define OSD_FLAGS_OSD_MSP_DEVICE        (1 << 6)
 
         uint8_t osdFlags = 0;
-#if defined(USE_OSD)
+
         osdFlags |= OSD_FLAGS_OSD_FEATURE;
 
         osdDisplayPortDevice_e deviceType;
@@ -979,7 +980,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
         default:
             break;
         }
-#endif
+
         sbufWriteU8(dst, osdFlags);
 
 #ifdef USE_OSD_SD
@@ -987,9 +988,8 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
         sbufWriteU8(dst, vcdProfile()->video_system);
 #else
         sbufWriteU8(dst, VIDEO_SYSTEM_HD);
-#endif
+#endif // USE_OSD_SD
 
-#ifdef USE_OSD
         // OSD specific, not applicable to OSD slaves.
 
         // Configuration
@@ -1052,9 +1052,9 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
         sbufWriteU8(dst, osdConfig()->camera_frame_width);
         sbufWriteU8(dst, osdConfig()->camera_frame_height);
 
-#endif // USE_OSD
         break;
     }
+#endif // USE_OSD
 
     case MSP_OSD_CANVAS: {
 #ifdef USE_OSD
@@ -1312,7 +1312,7 @@ case MSP_NAME:
             const uint8_t warningsLen = strlen(warningsBuffer);
 
             if (isBlinking) {
-                displayAttr |= DISPLAYPORT_ATTR_BLINK;
+                displayAttr |= DISPLAYPORT_BLINK;
             }
             sbufWriteU8(dst, displayAttr);  // see displayPortSeverity_e
             sbufWriteU8(dst, warningsLen);  // length byte followed by the actual characters
@@ -1538,7 +1538,7 @@ case MSP_NAME:
 
 #ifdef USE_GPS_RESCUE
     case MSP_GPS_RESCUE:
-        sbufWriteU16(dst, gpsRescueConfig()->angle);
+        sbufWriteU16(dst, gpsRescueConfig()->maxRescueAngle);
         sbufWriteU16(dst, gpsRescueConfig()->initialAltitudeM);
         sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
         sbufWriteU16(dst, gpsRescueConfig()->rescueGroundspeed);
@@ -2522,7 +2522,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
             // type byte, then length byte followed by the actual characters
             const uint8_t textType = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
 
-            char* textVar;
+            const char *textVar;
 
             switch (textType) {
                 case MSP2TEXT_PILOT_NAME:
@@ -2541,10 +2541,20 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
                     textVar = currentControlRateProfile->profileName;
                     break;
 
+                case MSP2TEXT_BUILDKEY:
+                    textVar = buildKey;
+                    break;
+
+                case MSP2TEXT_RELEASENAME:
+                    textVar = releaseName;
+                    break;
+
                 default:
                     return MSP_RESULT_ERROR;
             }
 
+            if (!textVar) return MSP_RESULT_ERROR;
+
             const uint8_t textLength = strlen(textVar);
 
             //  type byte, then length byte followed by the actual characters
@@ -2808,7 +2818,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
 
 #ifdef USE_GPS_RESCUE
         case MSP_SET_GPS_RESCUE:
-        gpsRescueConfigMutable()->angle = sbufReadU16(src);
+        gpsRescueConfigMutable()->maxRescueAngle = sbufReadU16(src);
         gpsRescueConfigMutable()->initialAltitudeM = sbufReadU16(src);
         gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
         gpsRescueConfigMutable()->rescueGroundspeed = sbufReadU16(src);
@@ -3565,7 +3575,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         }
         break;
 
-#ifdef USE_FLASHFS
+#if defined(USE_FLASHFS) && defined(USE_BLACKBOX)
     case MSP_DATAFLASH_ERASE:
         blackboxEraseAll();
 
@@ -3573,6 +3583,34 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
 #endif
 
 #ifdef USE_GPS
+    case MSP2_SENSOR_GPS:
+        (void)sbufReadU8(src);              // instance
+        (void)sbufReadU16(src);             // gps_week
+        (void)sbufReadU32(src);     // ms_tow
+        gpsSetFixState(sbufReadU8(src) != 0); // fix_type
+        gpsSol.numSat = sbufReadU8(src);    // satellites_in_view
+        gpsSol.acc.hAcc = sbufReadU16(src) * 10; // horizontal_pos_accuracy - convert cm to mm
+        gpsSol.acc.vAcc = sbufReadU16(src) * 10; // vertical_pos_accuracy - convert cm to mm
+        gpsSol.acc.sAcc = sbufReadU16(src) * 10; // horizontal_vel_accuracy - convert cm to mm
+        gpsSol.dop.hdop = sbufReadU16(src); // hdop
+        gpsSol.llh.lon = sbufReadU32(src);
+        gpsSol.llh.lat = sbufReadU32(src);
+        gpsSol.llh.altCm = sbufReadU32(src); // alt
+        int32_t ned_vel_north = (int32_t)sbufReadU32(src);  // ned_vel_north
+        int32_t ned_vel_east = (int32_t)sbufReadU32(src);   // ned_vel_east
+        gpsSol.groundSpeed = (uint16_t)sqrtf((ned_vel_north * ned_vel_north) + (ned_vel_east * ned_vel_east));
+        (void)sbufReadU32(src);             // ned_vel_down
+        gpsSol.groundCourse = ((uint16_t)sbufReadU16(src) % 360);   // ground_course
+        (void)sbufReadU16(src);             // true_yaw
+        (void)sbufReadU16(src);             // year
+        (void)sbufReadU8(src);              // month
+        (void)sbufReadU8(src);              // day
+        (void)sbufReadU8(src);              // hour
+        (void)sbufReadU8(src);              // min
+        (void)sbufReadU8(src);              // sec
+        GPS_update |= GPS_MSP_UPDATE;        // MSP data signalisation to GPS functions
+        break;
+
     case MSP_SET_RAW_GPS:
         gpsSetFixState(sbufReadU8(src));
         gpsSol.numSat = sbufReadU8(src);
diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/src/main/msp/msp_protocol_v2_betaflight.h
index f59d4a61289..a37e7b39dc7 100644
--- a/src/main/msp/msp_protocol_v2_betaflight.h
+++ b/src/main/msp/msp_protocol_v2_betaflight.h
@@ -32,3 +32,5 @@
 #define MSP2TEXT_CRAFT_NAME                      2
 #define MSP2TEXT_PID_PROFILE_NAME                3
 #define MSP2TEXT_RATE_PROFILE_NAME               4
+#define MSP2TEXT_BUILDKEY                        5
+#define MSP2TEXT_RELEASENAME                     6
diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h
index 2031df2bbef..1f2f42a42cf 100644
--- a/src/main/msp/msp_protocol_v2_common.h
+++ b/src/main/msp/msp_protocol_v2_common.h
@@ -20,3 +20,6 @@
 
 #define MSP2_COMMON_SERIAL_CONFIG       0x1009
 #define MSP2_COMMON_SET_SERIAL_CONFIG   0x100A
+
+// Sensors
+#define MSP2_SENSOR_GPS                 0x1F03
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 08b1c0ba6df..dbae6876723 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -69,6 +69,8 @@ void mspSerialAllocatePorts(void)
 
         if (mspConfig()->halfDuplex) {
             options |= SERIAL_BIDIR;
+        } else if ((portConfig->identifier >= SERIAL_PORT_USART1) && (portConfig->identifier <= SERIAL_PORT_USART10)){
+            options |= SERIAL_CHECK_TX;
         }
 
         serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, options);
diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c
index 37b339468bb..57c3ceeb2e3 100644
--- a/src/main/osd/osd.c
+++ b/src/main/osd/osd.c
@@ -449,7 +449,7 @@ static void osdDrawLogo(int x, int y)
     for (int row = 0; row < OSD_LOGO_ROWS; row++) {
         for (int column = 0; column < OSD_LOGO_COLS; column++) {
             if (fontOffset <= SYM_END_OF_FONT)
-                displayWriteChar(osdDisplayPort, x + column, y + row, DISPLAYPORT_ATTR_NORMAL, fontOffset++);
+                displayWriteChar(osdDisplayPort, x + column, y + row, DISPLAYPORT_SEVERITY_NORMAL, fontOffset++);
         }
     }
 }
@@ -473,17 +473,17 @@ static void osdCompleteInitialization(void)
 
     char string_buffer[30];
     tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
-    displayWrite(osdDisplayPort, midCol + 5, midRow, DISPLAYPORT_ATTR_NORMAL, string_buffer);
+    displayWrite(osdDisplayPort, midCol + 5, midRow, DISPLAYPORT_SEVERITY_NORMAL, string_buffer);
 #ifdef USE_CMS
-    displayWrite(osdDisplayPort, midCol - 8, midRow + 2,  DISPLAYPORT_ATTR_NORMAL, CMS_STARTUP_HELP_TEXT1);
-    displayWrite(osdDisplayPort, midCol - 4, midRow + 3, DISPLAYPORT_ATTR_NORMAL, CMS_STARTUP_HELP_TEXT2);
-    displayWrite(osdDisplayPort, midCol - 4, midRow + 4, DISPLAYPORT_ATTR_NORMAL, CMS_STARTUP_HELP_TEXT3);
+    displayWrite(osdDisplayPort, midCol - 8, midRow + 2,  DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT1);
+    displayWrite(osdDisplayPort, midCol - 4, midRow + 3, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT2);
+    displayWrite(osdDisplayPort, midCol - 4, midRow + 4, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT3);
 #endif
 
 #ifdef USE_RTC_TIME
     char dateTimeBuffer[FORMATTED_DATE_TIME_BUFSIZE];
     if (osdFormatRtcDateTime(&dateTimeBuffer[0])) {
-        displayWrite(osdDisplayPort, midCol - 10, midRow + 6, DISPLAYPORT_ATTR_NORMAL, dateTimeBuffer);
+        displayWrite(osdDisplayPort, midCol - 10, midRow + 6, DISPLAYPORT_SEVERITY_NORMAL, dateTimeBuffer);
     }
 #endif
 
@@ -732,9 +732,9 @@ static void osdGetBlackboxStatusString(char * buff)
 
 static void osdDisplayStatisticLabel(uint8_t x, uint8_t y, const char * text, const char * value)
 {
-    displayWrite(osdDisplayPort, x - 13, y, DISPLAYPORT_ATTR_NORMAL, text);
-    displayWrite(osdDisplayPort, x + 5, y, DISPLAYPORT_ATTR_NORMAL, ":");
-    displayWrite(osdDisplayPort, x + 7, y, DISPLAYPORT_ATTR_NORMAL, value);
+    displayWrite(osdDisplayPort, x - 13, y, DISPLAYPORT_SEVERITY_NORMAL, text);
+    displayWrite(osdDisplayPort, x + 5, y, DISPLAYPORT_SEVERITY_NORMAL, ":");
+    displayWrite(osdDisplayPort, x + 7, y, DISPLAYPORT_SEVERITY_NORMAL, value);
 }
 
 /*
@@ -767,7 +767,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
             tfp_sprintf(buff, "NO RTC");
         }
 
-        displayWrite(osdDisplayPort, midCol - 13, displayRow, DISPLAYPORT_ATTR_NORMAL, buff);
+        displayWrite(osdDisplayPort, midCol - 13, displayRow, DISPLAYPORT_SEVERITY_NORMAL, buff);
         return true;
     }
 
@@ -1021,7 +1021,7 @@ static bool osdRenderStatsContinue(void)
         }
 
         if (displayLabel) {
-            displayWrite(osdDisplayPort, midCol - (strlen("--- STATS ---") / 2), osdStatsRenderingState.row++, DISPLAYPORT_ATTR_NORMAL, "--- STATS ---");
+            displayWrite(osdDisplayPort, midCol - (strlen("--- STATS ---") / 2), osdStatsRenderingState.row++, DISPLAYPORT_SEVERITY_NORMAL, "--- STATS ---");
             return false;
         }
     }
@@ -1128,10 +1128,10 @@ static timeDelta_t osdShowArmed(void)
     } else {
         ret = (REFRESH_1S / 2);
     }
-    displayWrite(osdDisplayPort, midCol - (strlen("ARMED") / 2), midRow, DISPLAYPORT_ATTR_NORMAL, "ARMED");
+    displayWrite(osdDisplayPort, midCol - (strlen("ARMED") / 2), midRow, DISPLAYPORT_SEVERITY_NORMAL, "ARMED");
 
     if (isFlipOverAfterCrashActive()) {
-        displayWrite(osdDisplayPort, midCol - (strlen(CRASH_FLIP_WARNING) / 2), midRow + 1, DISPLAYPORT_ATTR_NORMAL, CRASH_FLIP_WARNING);
+        displayWrite(osdDisplayPort, midCol - (strlen(CRASH_FLIP_WARNING) / 2), midRow + 1, DISPLAYPORT_SEVERITY_NORMAL, CRASH_FLIP_WARNING);
     }
 
     return ret;
diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c
index 279299e0104..e5f022791f6 100644
--- a/src/main/osd/osd_elements.c
+++ b/src/main/osd/osd_elements.c
@@ -239,7 +239,7 @@ enum {UP, DOWN};
 static int osdDisplayWrite(osdElementParms_t *element, uint8_t x, uint8_t y, uint8_t attr, const char *s)
 {
     if (IS_BLINK(element->item)) {
-        attr |= DISPLAYPORT_ATTR_BLINK;
+        attr |= DISPLAYPORT_BLINK;
     }
 
     return displayWrite(element->osdDisplayPort, x, y, attr, s);
@@ -287,7 +287,7 @@ static void renderOsdEscRpmOrFreq(getEscRpmOrFreqFnPtr escFnPtr, osdElementParms
         const int rpm = MIN((*escFnPtr)(i),99999);
         const int len = tfp_sprintf(rpmStr, "%d", rpm);
         rpmStr[len] = '\0';
-        osdDisplayWrite(element, x, y + i, DISPLAYPORT_ATTR_NORMAL, rpmStr);
+        osdDisplayWrite(element, x, y + i, DISPLAYPORT_SEVERITY_NORMAL, rpmStr);
     }
     element->drawElement = false;
 }
@@ -666,7 +666,7 @@ static void osdElementAltitude(osdElementParms_t *element)
     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100;
 
     if ((alt >= osdConfig()->alt_alarm) && ARMING_FLAG(ARMED)) {
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
     }
 
     if (haveBaro || haveGps) {
@@ -713,7 +713,7 @@ static void osdElementArtificialHorizon(osdElementParms_t *element)
     for (int x = -4; x <= 4; x++) {
         const int y = ((-rollAngle * x) / 64) - pitchAngle;
         if (y >= 0 && y <= 81) {
-            osdDisplayWriteChar(element, element->elemPosX + x, element->elemPosY + (y / AH_SYMBOL_COUNT), DISPLAYPORT_ATTR_NORMAL, (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
+            osdDisplayWriteChar(element, element->elemPosX + x, element->elemPosY + (y / AH_SYMBOL_COUNT), DISPLAYPORT_SEVERITY_NORMAL, (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
         }
     }
 
@@ -743,7 +743,7 @@ static void osdElementUpDownReference(osdElementParms_t *element)
         int posX = element->elemPosX + lrintf(scaleRangef(psiB, -M_PIf / 4, M_PIf / 4, -14, 14));
         int posY = element->elemPosY + lrintf(scaleRangef(thetaB, -M_PIf / 4, M_PIf / 4, -8, 8));
 
-        osdDisplayWrite(element, posX, posY, DISPLAYPORT_ATTR_NORMAL, symbol[direction]);
+        osdDisplayWrite(element, posX, posY, DISPLAYPORT_SEVERITY_NORMAL, symbol[direction]);
     }
     element->drawElement = false;  // element already drawn
 }
@@ -756,10 +756,10 @@ static void osdElementAverageCellVoltage(osdElementParms_t *element)
 
     switch (batteryState) {
     case BATTERY_WARNING:
-        element->attr = DISPLAYPORT_ATTR_WARNING;
+        element->attr = DISPLAYPORT_SEVERITY_WARNING;
         break;
     case BATTERY_CRITICAL:
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
         break;
     default:
         break;
@@ -795,12 +795,12 @@ static void osdBackgroundCameraFrame(osdElementParms_t *element)
     element->buff[width - 1] = SYM_STICK_OVERLAY_CENTER;
     element->buff[width] = 0;  // string terminator
 
-    osdDisplayWrite(element, xpos, ypos, DISPLAYPORT_ATTR_NORMAL, element->buff);
+    osdDisplayWrite(element, xpos, ypos, DISPLAYPORT_SEVERITY_NORMAL, element->buff);
     for (int i = 1; i < (height - 1); i++) {
-        osdDisplayWriteChar(element, xpos, ypos + i, DISPLAYPORT_ATTR_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
-        osdDisplayWriteChar(element, xpos + width - 1, ypos + i, DISPLAYPORT_ATTR_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
+        osdDisplayWriteChar(element, xpos, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
+        osdDisplayWriteChar(element, xpos + width - 1, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
     }
-    osdDisplayWrite(element, xpos, ypos + height - 1, DISPLAYPORT_ATTR_NORMAL, element->buff);
+    osdDisplayWrite(element, xpos, ypos + height - 1, DISPLAYPORT_SEVERITY_NORMAL, element->buff);
 
     element->drawElement = false;  // element already drawn
 }
@@ -1080,15 +1080,15 @@ static void osdElementGpsCoordinate(osdElementParms_t *element)
 static void osdElementGpsSats(osdElementParms_t *element)
 {
     if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < GPS_MIN_SAT_COUNT) ) {
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
     }
 #ifdef USE_GPS_RESCUE
     else if ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured()) {
-        element->attr = DISPLAYPORT_ATTR_WARNING;
+        element->attr = DISPLAYPORT_SEVERITY_WARNING;
     }
 #endif
     else {
-        element->attr = DISPLAYPORT_ATTR_INFO;
+        element->attr = DISPLAYPORT_SEVERITY_INFO;
     }
 
     if (!gpsIsHealthy()) {
@@ -1135,13 +1135,13 @@ static void osdBackgroundHorizonSidebars(osdElementParms_t *element)
     const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
     const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
     for (int y = -hudheight; y <= hudheight; y++) {
-        osdDisplayWriteChar(element, element->elemPosX - hudwidth, element->elemPosY + y, DISPLAYPORT_ATTR_NORMAL, SYM_AH_DECORATION);
-        osdDisplayWriteChar(element, element->elemPosX + hudwidth, element->elemPosY + y, DISPLAYPORT_ATTR_NORMAL, SYM_AH_DECORATION);
+        osdDisplayWriteChar(element, element->elemPosX - hudwidth, element->elemPosY + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_DECORATION);
+        osdDisplayWriteChar(element, element->elemPosX + hudwidth, element->elemPosY + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_DECORATION);
     }
 
     // AH level indicators
-    osdDisplayWriteChar(element, element->elemPosX - hudwidth + 1, element->elemPosY, DISPLAYPORT_ATTR_NORMAL, SYM_AH_LEFT);
-    osdDisplayWriteChar(element, element->elemPosX + hudwidth - 1, element->elemPosY, DISPLAYPORT_ATTR_NORMAL, SYM_AH_RIGHT);
+    osdDisplayWriteChar(element, element->elemPosX - hudwidth + 1, element->elemPosY, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_LEFT);
+    osdDisplayWriteChar(element, element->elemPosX + hudwidth - 1, element->elemPosY, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_RIGHT);
 
     element->drawElement = false;  // element already drawn
 }
@@ -1152,7 +1152,7 @@ static void osdElementLinkQuality(osdElementParms_t *element)
     uint16_t osdLinkQuality = 0;
 
     if (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm) {
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
     }
 
     if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) { // 0-99
@@ -1209,7 +1209,7 @@ static void osdElementMahDrawn(osdElementParms_t *element)
     const int mAhDrawn = getMAhDrawn();
 
     if (mAhDrawn >= osdConfig()->cap_alarm) {
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
     }
 
     tfp_sprintf(element->buff, "%4d%c", mAhDrawn, SYM_MAH);
@@ -1221,7 +1221,7 @@ static void osdElementWattHoursDrawn(osdElementParms_t *element)
     const float wattHoursDrawn = getWhDrawn();
 
     if (mAhDrawn >= osdConfig()->cap_alarm) {
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
     }
 
     if (wattHoursDrawn < 1.0f) {        
@@ -1243,7 +1243,7 @@ static void osdElementMainBatteryUsage(osdElementParms_t *element)
     int displayBasis = usedCapacity;
 
     if (mAhDrawn >= osdConfig()->cap_alarm) {
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
     }
 
     switch (element->type) {
@@ -1298,10 +1298,10 @@ static void osdElementMainBatteryVoltage(osdElementParms_t *element)
 
     switch (batteryState) {
     case BATTERY_WARNING:
-        element->attr = DISPLAYPORT_ATTR_WARNING;
+        element->attr = DISPLAYPORT_SEVERITY_WARNING;
         break;
     case BATTERY_CRITICAL:
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
         break;
     default:
         break;
@@ -1402,7 +1402,7 @@ static void osdElementRcChannels(osdElementParms_t *element)
             // Decimal notation can be added when tfp_sprintf supports float among fancy options.
             char fmtbuf[6];
             tfp_sprintf(fmtbuf, "%5d", data);
-            osdDisplayWrite(element, xpos, ypos + i, DISPLAYPORT_ATTR_NORMAL, fmtbuf);
+            osdDisplayWrite(element, xpos, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, fmtbuf);
         }
     }
 
@@ -1414,7 +1414,7 @@ static void osdElementRemainingTimeEstimate(osdElementParms_t *element)
     const int mAhDrawn = getMAhDrawn();
 
     if (mAhDrawn >= osdConfig()->cap_alarm) {
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
     }
 
     if (mAhDrawn <= 0.1 * osdConfig()->cap_alarm) {  // also handles the mAhDrawn == 0 condition
@@ -1435,7 +1435,7 @@ static void osdElementRssi(osdElementParms_t *element)
     }
 
     if (getRssiPercent() < osdConfig()->rssi_alarm) {
-        element->attr = DISPLAYPORT_ATTR_CRITICAL;
+        element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
     }
 
     tfp_sprintf(element->buff, "%c%2d", SYM_RSSI, osdRssi);
@@ -1473,11 +1473,11 @@ static void osdBackgroundStickOverlay(osdElementParms_t *element)
         for (unsigned  y = 0; y < OSD_STICK_OVERLAY_HEIGHT; y++) {
             // draw the axes, vertical and horizonal
             if ((x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) && (y == (OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
-                osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_ATTR_NORMAL, SYM_STICK_OVERLAY_CENTER);
+                osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_CENTER);
             } else if (x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) {
-                osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_ATTR_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
+                osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
             } else if (y == ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
-                osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_ATTR_NORMAL, SYM_STICK_OVERLAY_HORIZONTAL);
+                osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_HORIZONTAL);
             }
         }
     }
@@ -1505,7 +1505,7 @@ static void osdElementStickOverlay(osdElementParms_t *element)
     const uint8_t cursorY = OSD_STICK_OVERLAY_VERTICAL_POSITIONS - 1 - scaleRange(constrain(rcData[vertical_channel], PWM_RANGE_MIN, PWM_RANGE_MAX - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_VERTICAL_POSITIONS);
     const char cursor = SYM_STICK_OVERLAY_SPRITE_HIGH + (cursorY % OSD_STICK_OVERLAY_SPRITE_HEIGHT);
 
-    osdDisplayWriteChar(element, xpos + cursorX, ypos + cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT, DISPLAYPORT_ATTR_NORMAL, cursor);
+    osdDisplayWriteChar(element, xpos + cursorX, ypos + cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT, DISPLAYPORT_SEVERITY_NORMAL, cursor);
 
     element->drawElement = false;  // element already drawn
 }
@@ -1523,7 +1523,7 @@ static void osdElementTimer(osdElementParms_t *element)
         const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
         const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
         if (alarmTime != 0 && time >= alarmTime) {
-            element->attr = DISPLAYPORT_ATTR_CRITICAL;
+            element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
         }
     }
 
@@ -1951,7 +1951,7 @@ static void osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
     element.buff = (char *)&buff;
     element.osdDisplayPort = osdDisplayPort;
     element.drawElement = true;
-    element.attr = DISPLAYPORT_ATTR_NORMAL;
+    element.attr = DISPLAYPORT_SEVERITY_NORMAL;
 
     // Call the element drawing function
     if ((item >= OSD_SYS_GOGGLE_VOLTAGE) && (item < OSD_ITEM_COUNT)) {
@@ -1987,7 +1987,7 @@ static void osdDrawSingleElementBackground(displayPort_t *osdDisplayPort, uint8_
     // Call the element background drawing function
     osdElementBackgroundFunction[item](&element);
     if (element.drawElement) {
-        osdDisplayWrite(&element, elemPosX, elemPosY, DISPLAYPORT_ATTR_NORMAL, buff);
+        osdDisplayWrite(&element, elemPosX, elemPosY, DISPLAYPORT_SEVERITY_NORMAL, buff);
     }
 }
 
diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c
index 031df5a967e..a1dbcce2b99 100644
--- a/src/main/osd/osd_warnings.c
+++ b/src/main/osd/osd_warnings.c
@@ -75,7 +75,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     static unsigned armingDisabledDisplayIndex;
 
     warningText[0] = '\0';
-    *displayAttr = DISPLAYPORT_ATTR_NORMAL;
+    *displayAttr = DISPLAYPORT_SEVERITY_NORMAL;
     *blinking = false;
 
     // Cycle through the arming disabled reasons
@@ -105,7 +105,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
             }
 
             tfp_sprintf(warningText, "%s", armingDisableFlagNames[armingDisabledDisplayIndex]);
-            *displayAttr = DISPLAYPORT_ATTR_WARNING;
+            *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
             return;
         } else {
             armingDisabledUpdateTimeUs = 0;
@@ -123,13 +123,13 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
         } else {
             tfp_sprintf(warningText, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10);
         }
-        *displayAttr = DISPLAYPORT_ATTR_INFO;
+        *displayAttr = DISPLAYPORT_SEVERITY_INFO;
         return;
     }
 #endif // USE_DSHOT
     if (osdWarnGetState(OSD_WARNING_FAIL_SAFE) && failsafeIsActive()) {
         tfp_sprintf(warningText, "FAIL SAFE");
-        *displayAttr = DISPLAYPORT_ATTR_CRITICAL;
+        *displayAttr = DISPLAYPORT_SEVERITY_CRITICAL;
         *blinking = true;
         return;
     }
@@ -138,11 +138,11 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
         if (isFlipOverAfterCrashActive()) { // if was armed in crash flip mode
             tfp_sprintf(warningText, CRASH_FLIP_WARNING);
-            *displayAttr = DISPLAYPORT_ATTR_INFO;
+            *displayAttr = DISPLAYPORT_SEVERITY_INFO;
             return;
         } else if (!ARMING_FLAG(ARMED)) { // if disarmed, but crash flip mode is activated
             tfp_sprintf(warningText, "CRASH FLIP SWITCH");
-            *displayAttr = DISPLAYPORT_ATTR_INFO;
+            *displayAttr = DISPLAYPORT_SEVERITY_INFO;
             return;
         }
     }
@@ -165,7 +165,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
             *blinking = true;
         }
 
-        *displayAttr = DISPLAYPORT_ATTR_INFO;
+        *displayAttr = DISPLAYPORT_SEVERITY_INFO;
         return;
     }
 #endif // USE_LAUNCH_CONTROL
@@ -173,7 +173,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     // RSSI
     if (osdWarnGetState(OSD_WARNING_RSSI) && (getRssiPercent() < osdConfig()->rssi_alarm)) {
         tfp_sprintf(warningText, "RSSI LOW");
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -181,7 +181,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     // rssi dbm
     if (osdWarnGetState(OSD_WARNING_RSSI_DBM) && (getRssiDbm() < osdConfig()->rssi_dbm_alarm)) {
         tfp_sprintf(warningText, "RSSI DBM");
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -190,7 +190,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     // rsnr
     if (osdWarnGetState(OSD_WARNING_RSNR) && (getRsnr() < osdConfig()->rsnr_alarm)) {
         tfp_sprintf(warningText, "RSNR LOW");
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -200,7 +200,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     // Link Quality
     if (osdWarnGetState(OSD_WARNING_LINK_QUALITY) && (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm)) {
         tfp_sprintf(warningText, "LINK QUALITY");
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -208,7 +208,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
 
     if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) {
         tfp_sprintf(warningText, " LAND NOW");
-        *displayAttr = DISPLAYPORT_ATTR_CRITICAL;
+        *displayAttr = DISPLAYPORT_SEVERITY_CRITICAL;
         *blinking = true;
         return;
     }
@@ -220,7 +220,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
        !gpsRescueIsDisabled() &&
        !gpsRescueIsAvailable()) {
         tfp_sprintf(warningText, "RESCUE N/A");
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -233,7 +233,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
         statistic_t *stats = osdGetStats();
         if (cmpTimeUs(stats->armed_time, OSD_GPS_RESCUE_DISABLED_WARNING_DURATION_US) < 0) {
             tfp_sprintf(warningText, "RESCUE OFF");
-            *displayAttr = DISPLAYPORT_ATTR_WARNING;
+            *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
             *blinking = true;
             return;
         }
@@ -244,7 +244,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     // Show warning if in HEADFREE flight mode
     if (FLIGHT_MODE(HEADFREE_MODE)) {
         tfp_sprintf(warningText, "HEADFREE");
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -253,7 +253,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     const int16_t coreTemperature = getCoreTemperatureCelsius();
     if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) {
         tfp_sprintf(warningText, "CORE %c: %3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(coreTemperature), osdGetTemperatureSymbolForSelectedUnit());
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -305,7 +305,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
 
         if (escWarningCount > 0) {
             tfp_sprintf(warningText, "%s", escWarningMsg);
-            *displayAttr = DISPLAYPORT_ATTR_WARNING;
+            *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
             *blinking = true;
             return;
         }
@@ -361,7 +361,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
         // If warning exists then notify, otherwise clear warning message
         if (dshotEscErrorLength > 3) {
             warningText[dshotEscErrorLength] = 0;        // End string
-            *displayAttr = DISPLAYPORT_ATTR_WARNING;
+            *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
             *blinking = true;
             return;
         } else {
@@ -372,7 +372,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
 
     if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
         tfp_sprintf(warningText, "LOW BATTERY");
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -381,7 +381,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     // Show warning if rc smoothing hasn't initialized the filters
     if (osdWarnGetState(OSD_WARNING_RC_SMOOTHING) && ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) {
         tfp_sprintf(warningText, "RCSMOOTHING");
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -390,7 +390,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     // Show warning if mah consumed is over the configured limit
     if (osdWarnGetState(OSD_WARNING_OVER_CAP) && ARMING_FLAG(ARMED) && osdConfig()->cap_alarm > 0 && getMAhDrawn() >= osdConfig()->cap_alarm) {
         tfp_sprintf(warningText, "OVER CAP");
-        *displayAttr = DISPLAYPORT_ATTR_WARNING;
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
         *blinking = true;
         return;
     }
@@ -399,7 +399,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     // Show warning if battery is not fresh and battery continue is active
     if (hasUsedMAh()) {
         tfp_sprintf(warningText, "BATTERY CONTINUE");
-        *displayAttr = DISPLAYPORT_ATTR_INFO;
+        *displayAttr = DISPLAYPORT_SEVERITY_INFO;
         return;
     }
 #endif // USE_BATTERY_CONTINUE
@@ -408,14 +408,14 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
     if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !(ARMING_FLAG(ARMED) || ARMING_FLAG(WAS_EVER_ARMED)) && (getBatteryState() == BATTERY_OK)
           && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
         tfp_sprintf(warningText, "BATT < FULL");
-        *displayAttr = DISPLAYPORT_ATTR_INFO;
+        *displayAttr = DISPLAYPORT_SEVERITY_INFO;
         return;
     }
 
     // Visual beeper
     if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && osdGetVisualBeeperState()) {
         tfp_sprintf(warningText, "  * * * *");
-        *displayAttr = DISPLAYPORT_ATTR_INFO;
+        *displayAttr = DISPLAYPORT_SEVERITY_INFO;
         osdSetVisualBeeperState(false);
         return;
     }
diff --git a/src/main/pg/displayport_profiles.h b/src/main/pg/displayport_profiles.h
index 585d6fc412f..22a7968c00d 100644
--- a/src/main/pg/displayport_profiles.h
+++ b/src/main/pg/displayport_profiles.h
@@ -32,7 +32,7 @@ typedef struct displayPortProfile_s {
 
     // For attribute-rich OSDs
 
-    uint8_t fontSelection[DISPLAYPORT_ATTR_COUNT];
+    uint8_t fontSelection[DISPLAYPORT_SEVERITY_COUNT];
     uint8_t useDeviceBlink;    // Use device local blink capability
 } displayPortProfile_t;
 
diff --git a/src/main/pg/flash.c b/src/main/pg/flash.c
index 1b3b8b4297b..e28b5707447 100644
--- a/src/main/pg/flash.c
+++ b/src/main/pg/flash.c
@@ -27,6 +27,7 @@
 
 #include "drivers/bus_spi.h"
 #include "drivers/bus_quadspi.h"
+#include "drivers/bus_octospi.h"
 #include "drivers/io.h"
 
 #include "pg/pg.h"
@@ -42,12 +43,17 @@ PG_REGISTER_WITH_RESET_FN(flashConfig_t, flashConfig, PG_FLASH_CONFIG, 0);
 
 void pgResetFn_flashConfig(flashConfig_t *flashConfig)
 {
+    // CS pin can be used by all IO interfaces, not just SPI.
     flashConfig->csTag = IO_TAG(FLASH_CS_PIN);
-#if defined(USE_SPI) && defined(FLASH_SPI_INSTANCE)
+
+#if defined(USE_FLASH_SPI) && defined(FLASH_SPI_INSTANCE)
     flashConfig->spiDevice = SPI_DEV_TO_CFG(spiDeviceByInstance(FLASH_SPI_INSTANCE));
 #endif
-#if defined(USE_QUADSPI) && defined(FLASH_QUADSPI_INSTANCE)
+#if defined(USE_FLASH_QUADSPI) && defined(FLASH_QUADSPI_INSTANCE)
     flashConfig->quadSpiDevice = QUADSPI_DEV_TO_CFG(quadSpiDeviceByInstance(FLASH_QUADSPI_INSTANCE));
 #endif
+#if defined(USE_FLASH_OCTOSPI) && defined(FLASH_OCTOSPI_INSTANCE)
+    flashConfig->octoSpiDevice = OCTOSPI_DEV_TO_CFG(octoSpiDeviceByInstance(FLASH_OCTOSPI_INSTANCE));
+#endif
 }
 #endif
diff --git a/src/main/pg/flash.h b/src/main/pg/flash.h
index 20fc618b5a5..96b0bb76439 100644
--- a/src/main/pg/flash.h
+++ b/src/main/pg/flash.h
@@ -30,6 +30,7 @@ typedef struct flashConfig_s {
     ioTag_t csTag;
     uint8_t spiDevice;
     uint8_t quadSpiDevice;
+    uint8_t octoSpiDevice;
 } flashConfig_t;
 
 PG_DECLARE(flashConfig_t, flashConfig);
diff --git a/src/main/pg/gps_rescue.c b/src/main/pg/gps_rescue.c
index d0ade463fe4..0bfeba3fa04 100644
--- a/src/main/pg/gps_rescue.c
+++ b/src/main/pg/gps_rescue.c
@@ -29,26 +29,28 @@
 
 #include "gps_rescue.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 4);
 
 PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
 
-    .minRescueDth = 30,
+    .minRescueDth = 15,
     .altitudeMode = GPS_RESCUE_ALT_MODE_MAX,
     .rescueAltitudeBufferM = 10,
-    .ascendRate = 500,          // cm/s, for altitude corrections on ascent
+    .ascendRate = 750,          // cm/s, for altitude corrections on ascent
 
     .initialAltitudeM = 30,
-    .rescueGroundspeed = 500,
-    .angle = 40,
+    .rescueGroundspeed = 750,
+    .maxRescueAngle = 45,
     .rollMix = 150,
+    .pitchCutoffHz = 75,
 
     .descentDistanceM = 20,
-    .descendRate = 100,         // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
+    .descendRate = 150,         // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
     .targetLandingAltitudeM = 4,
+    .disarmThreshold = 20,
 
     .throttleMin = 1100,
-    .throttleMax = 1600,
+    .throttleMax = 1700,
     .throttleHover = 1275,
 
     .allowArmingWithoutFix = false,
@@ -57,10 +59,10 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
 
     .throttleP = 15,
     .throttleI = 15,
-    .throttleD = 15,
+    .throttleD = 20,
     .velP = 8,
-    .velI = 30,
-    .velD = 20,
+    .velI = 40,
+    .velD = 12,
     .yawP = 20,
 
     .useMag = GPS_RESCUE_USE_MAG
diff --git a/src/main/pg/gps_rescue.h b/src/main/pg/gps_rescue.h
index 0a78dbbc892..4d9bed050d9 100644
--- a/src/main/pg/gps_rescue.h
+++ b/src/main/pg/gps_rescue.h
@@ -26,7 +26,7 @@
 
 typedef struct gpsRescue_s {
 
-    uint16_t angle; // degrees
+    uint16_t maxRescueAngle; // degrees
     uint16_t initialAltitudeM; // meters
     uint16_t descentDistanceM; // meters
     uint16_t rescueGroundspeed; // centimeters per second
@@ -47,6 +47,8 @@ typedef struct gpsRescue_s {
     uint16_t descendRate;
     uint16_t rescueAltitudeBufferM; // meters
     uint8_t  rollMix;
+    uint8_t  disarmThreshold;
+    uint8_t  pitchCutoffHz;
 
 } gpsRescueConfig_t;
 
diff --git a/src/main/pg/motor.c b/src/main/pg/motor.c
index 3f70786fd0c..a8493f27a27 100644
--- a/src/main/pg/motor.c
+++ b/src/main/pg/motor.c
@@ -33,7 +33,19 @@
 #include "pg/pg_ids.h"
 #include "pg/motor.h"
 
-PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);
+#if !defined(DEFAULT_DSHOT_BITBANG)
+#define DEFAULT_DSHOT_BITBANG DSHOT_BITBANG_AUTO
+#endif
+
+#if !defined(DSHOT_BITBANGED_TIMER_DEFAULT)
+#define DSHOT_BITBANGED_TIMER_DEFAULT DSHOT_BITBANGED_TIMER_AUTO
+#endif
+
+#if !defined(DEFAULT_DSHOT_BURST)
+#define DEFAULT_DSHOT_BURST DSHOT_DMAR_OFF
+#endif
+
+PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 2);
 
 void pgResetFn_motorConfig(motorConfig_t *motorConfig)
 {
diff --git a/src/main/pg/sdcard.c b/src/main/pg/sdcard.c
index c19b71ee81f..292203cb8f8 100644
--- a/src/main/pg/sdcard.c
+++ b/src/main/pg/sdcard.c
@@ -30,6 +30,7 @@
 
 #include "sdcard.h"
 #include "drivers/bus_spi.h"
+#include "drivers/sdio.h"
 #include "drivers/io.h"
 #include "drivers/dma.h"
 #include "drivers/dma_reqmap.h"
@@ -71,5 +72,11 @@ void pgResetFn_sdcardConfig(sdcardConfig_t *config)
         config->mode = SDCARD_MODE_SPI;
     }
 #endif
+
+#if defined(USE_SDCARD_SDIO)
+    if (SDIO_DEVICE != SDIOINVALID) {
+        config->mode = SDCARD_MODE_SDIO;
+    }
+#endif
 }
 #endif
diff --git a/src/main/platform.h b/src/main/platform.h
index 3b3e19dd441..2ff0012a449 100644
--- a/src/main/platform.h
+++ b/src/main/platform.h
@@ -118,10 +118,6 @@
 
 #include "target/common_pre.h"
 
-#ifdef __BOARD__
-#include "board.h"
-#endif
-
 #include "target.h"
 #include "target/common_post.h"
 #include "target/common_defaults_post.h"
diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c
index 0d0e7a90004..d0682cece4d 100644
--- a/src/main/rx/jetiexbus.c
+++ b/src/main/rx/jetiexbus.c
@@ -153,6 +153,7 @@ static void jetiExBusDataReceive(uint16_t c, void *data)
 
     static timeUs_t jetiExBusTimeLast = 0;
     static uint8_t *jetiExBusFrame;
+    static uint8_t jetiExBusFrameMaxSize;
     const timeUs_t now = microsISR();
 
     // Check if we shall reset frame position due to time
@@ -169,11 +170,13 @@ static void jetiExBusDataReceive(uint16_t c, void *data)
         case EXBUS_START_CHANNEL_FRAME:
             jetiExBusFrameState = EXBUS_STATE_IN_PROGRESS;
             jetiExBusFrame = jetiExBusChannelFrame;
+            jetiExBusFrameMaxSize = EXBUS_MAX_CHANNEL_FRAME_SIZE;
             break;
 
         case EXBUS_START_REQUEST_FRAME:
             jetiExBusRequestState = EXBUS_STATE_IN_PROGRESS;
             jetiExBusFrame = jetiExBusRequestFrame;
+            jetiExBusFrameMaxSize = EXBUS_MAX_REQUEST_FRAME_SIZE;
             break;
 
         default:
@@ -181,6 +184,15 @@ static void jetiExBusDataReceive(uint16_t c, void *data)
         }
     }
 
+    if (jetiExBusFramePosition == jetiExBusFrameMaxSize) {
+        // frame overrun
+        jetiExBusFrameReset();
+        jetiExBusFrameState = EXBUS_STATE_ZERO;
+        jetiExBusRequestState = EXBUS_STATE_ZERO;
+
+        return;
+    }
+
     // Store in frame copy
     jetiExBusFrame[jetiExBusFramePosition] = (uint8_t)c;
     jetiExBusFramePosition++;
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 2c117a40ab8..cbfef3c5931 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -599,7 +599,7 @@ static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample)
 static uint16_t getRxfailValue(uint8_t channel)
 {
     const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel);
-    const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
+    const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
 
     switch (channelFailsafeConfig->mode) {
     case RX_FAILSAFE_MODE_AUTO:
@@ -620,7 +620,7 @@ static uint16_t getRxfailValue(uint8_t channel)
     default:
     case RX_FAILSAFE_MODE_INVALID:
     case RX_FAILSAFE_MODE_HOLD:
-        if (failsafeAuxSwitch) {
+        if (boxFailsafeSwitchIsOn) {
             return rcRaw[channel]; // current values are allowed through on held channels with switch induced failsafe
         } else {
             return rcData[channel]; // last good value
@@ -671,41 +671,46 @@ static void readRxChannelsApplyRanges(void)
 void detectAndApplySignalLossBehaviour(void)
 {
     const uint32_t currentTimeMs = millis();
-    const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
-    rxFlightChannelsValid = rxSignalReceived && !failsafeAuxSwitch;
-    //  set rxFlightChannelsValid false when a packet is bad or we use a failsafe switch
+    const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
+    rxFlightChannelsValid = rxSignalReceived && !boxFailsafeSwitchIsOn;
+    // rxFlightChannelsValid is false after 100ms of no packets, or as soon as use the BOXFAILSAFE switch is actioned
+    // rxFlightChannelsValid is true the instant we get a good packet or the BOXFAILSAFE switch is reverted
+    // can also go false with good packets but where one flight channel is bad > 300ms (PPM type receiver error)
 
     for (int channel = 0; channel < rxChannelCount; channel++) {
         float sample = rcRaw[channel]; // sample has latest RC value, rcData has last 'accepted valid' value
         const bool thisChannelValid = rxFlightChannelsValid && isPulseValid(sample);
-        // if the whole packet is bad, consider all channels bad
-
+        // if the whole packet is bad, or BOXFAILSAFE switch is actioned, consider all channels bad
         if (thisChannelValid) {
             //  reset the invalid pulse period timer for every good channel
             validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME_MS;
         }
 
-        if (ARMING_FLAG(ARMED) && failsafeIsActive()) {
-            // while in failsafe Stage 2, whether Rx loss or switch induced, pass valid incoming flight channel values
-            // this allows GPS Rescue to detect the 30% requirement for termination
+        if (failsafeIsActive()) {
+            // we are in failsafe Stage 2, whether Rx loss or BOXFAILSAFE induced
+            // pass valid incoming flight channel values to FC,
+            // so that GPS Rescue can get the 30% requirement for termination of the rescue
             if (channel < NON_AUX_CHANNEL_COUNT) {
                 if (!thisChannelValid) {
                     if (channel == THROTTLE ) {
-                        sample = failsafeConfig()->failsafe_throttle; // stage 2 failsafe throttle value
+                        sample = failsafeConfig()->failsafe_throttle;
+                        // stage 2 failsafe throttle value. In GPS Rescue Flight mode, gpsRescueGetThrottle overrides, late in mixer.c
                     } else {
                         sample = rxConfig()->midrc;
                     }
                 }
             } else {
-                //  During Stage 2, set aux channels as per Stage 1 configuration
+                // set aux channels as per Stage 1 failsafe hold/set values, allow all for Failsafe and GPS rescue MODE switches
                 sample = getRxfailValue(channel);
             }
         } else {
-            if (failsafeAuxSwitch) {
+            // we are normal, or in failsafe stage 1
+            if (boxFailsafeSwitchIsOn) {
+                // BOXFAILSAFE active, but not in stage 2 yet, use stage 1 values
                 sample = getRxfailValue(channel);
                 //  set channels to Stage 1 values immediately failsafe switch is activated
             } else if (!thisChannelValid) {
-                // everything was normal and this channel was invalid
+                // everything is normal but this channel was invalid
                 if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
                     // first 300ms of Stage 1 failsafe
                     sample = rcData[channel];
@@ -720,6 +725,7 @@ void detectAndApplySignalLossBehaviour(void)
                     // set channels that are invalid for more than 300ms to Stage 1 values
                 }
             }
+            // everything is normal, ie rcData[channel] will be set to rcRaw(channel) via 'sample'
         }
 
         sample = constrainf(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
@@ -739,10 +745,10 @@ void detectAndApplySignalLossBehaviour(void)
 
     if (rxFlightChannelsValid) {
         failsafeOnValidDataReceived();
-        //  --> start the timer to exit stage 2 failsafe
+        //  --> start the timer to exit stage 2 failsafe 100ms after losing all packets or the BOXFAILSAFE switch is actioned
     } else {
         failsafeOnValidDataFailed();
-        //  -> start timer to enter stage2 failsafe
+        //  -> start timer to enter stage2 failsafe the instant we get a good packet or the BOXFAILSAFE switch is reverted
     }
 
     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index d92f2a32eb9..d3425ab8e0e 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -148,6 +148,7 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
 #define NUM_GROUND_LEVEL_CYCLES   10        // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 
 static uint16_t calibrationCycles = 0;      // baro calibration = get new ground pressure value
+static uint16_t calibrationCycleCount = 0;
 static float baroGroundAltitude = 0.0f;
 static bool baroCalibrated = false;
 static bool baroReady = false;
@@ -315,23 +316,20 @@ bool baroIsCalibrated(void)
     return baroCalibrated;
 }
 
-static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
-{
-    calibrationCycles = calibrationCyclesRequired;
-}
-
 void baroStartCalibration(void)
 {
-    baroSetCalibrationCycles(NUM_CALIBRATION_CYCLES);
     baroGroundAltitude = 0;
     baroCalibrated = false;
+    calibrationCycles = NUM_CALIBRATION_CYCLES;
+    calibrationCycleCount = 0;
 }
 
 void baroSetGroundLevel(void)
 {
-    baroSetCalibrationCycles(NUM_GROUND_LEVEL_CYCLES);
     baroGroundAltitude = 0;
     baroCalibrated = false;
+    calibrationCycles = NUM_GROUND_LEVEL_CYCLES;
+    calibrationCycleCount = 0;
 }
 
 typedef enum {
@@ -423,15 +421,23 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
 
             // update baro data
             baro.dev.calculate(&baro.pressure, &baro.temperature);
-            baro.altitude = pressureToAltitude(baro.pressure);
 
-            if (baroIsCalibrated()) {
-                // zero baro altitude
-                baro.altitude -= baroGroundAltitude;
+            // If baro.pressure is invalid then skip altitude counting / call of calibration cycle
+            if (baro.pressure > 0) {
+                const float altitude = pressureToAltitude(baro.pressure);
+                if (baroIsCalibrated()) {
+                    // zero baro altitude
+                    baro.altitude = altitude - baroGroundAltitude;
+                } else {
+                    // establish stable baroGroundAltitude value to zero baro altitude with
+                    performBaroCalibrationCycle(altitude);
+                    baro.altitude = 0.0f;
+                }
             } else {
-                // establish stable baroGroundAltitude value to zero baro altitude with
-                performBaroCalibrationCycle(baro.altitude);
-                baro.altitude = 0.0f;
+                // return 0 during calibration, reuse last value otherwise
+                if (!baroIsCalibrated()) {
+                    baro.altitude = 0.0f;
+                }
             }
 
             DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f));   // hPa
@@ -469,15 +475,13 @@ float getBaroAltitude(void)
 
 static void performBaroCalibrationCycle(const float altitude)
 {
-    static uint16_t cycleCount = 0;
-
     baroGroundAltitude += altitude;
-    cycleCount++;
+    calibrationCycleCount++;
 
-    if (cycleCount >= calibrationCycles) {
-        baroGroundAltitude /= cycleCount;  // simple average
+    if (calibrationCycleCount >= calibrationCycles) {
+        baroGroundAltitude /= calibrationCycleCount;  // simple average
         baroCalibrated = true;
-        cycleCount = 0;
+        calibrationCycleCount = 0;
     }
 }
 
diff --git a/src/main/startup/system_stm32h7xx.c b/src/main/startup/system_stm32h7xx.c
index 890ed7ee578..c22fc2f19b2 100644
--- a/src/main/startup/system_stm32h7xx.c
+++ b/src/main/startup/system_stm32h7xx.c
@@ -735,6 +735,12 @@ void SystemInit (void)
     systemProcessResetReason();
 #endif
 
+#if defined(USE_FLASH_MEMORY_MAPPED)
+    memoryMappedModeInit();
+
+    // !IMPORTANT!  Do NOT reset peripherals, clocks and GPIO pins used by the MCU to access the memory-mapped flash!!!
+#endif
+
     // FPU settings
 #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
     SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  // Set CP10 and CP11 Full Access
diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c
index 0a550430a11..24987e560c9 100644
--- a/src/main/target/SITL/target.c
+++ b/src/main/target/SITL/target.c
@@ -520,7 +520,7 @@ static motorDevice_t motorPwmDevice = {
         .enable = pwmEnableMotors,
         .disable = pwmDisableMotors,
         .isMotorEnabled = pwmIsMotorEnabled,
-        .updateStart = motorUpdateStartNull,
+        .decodeTelemetry = motorDecodeTelemetryNull,
         .write = pwmWriteMotor,
         .writeInt = pwmWriteMotorInt,
         .updateComplete = pwmCompleteMotorUpdate,
diff --git a/src/main/target/STM32F405/target.h b/src/main/target/STM32F405/target.h
index 684c5da42b2..7fd5155bbc4 100644
--- a/src/main/target/STM32F405/target.h
+++ b/src/main/target/STM32F405/target.h
@@ -57,6 +57,8 @@
 #define USE_I2C
 #define I2C_FULL_RECONFIGURABILITY
 
+#define USE_DSHOT_BITBAND
+
 #define USE_BEEPER
 
 #define USE_SPI
diff --git a/src/main/target/STM32F411/target.h b/src/main/target/STM32F411/target.h
index 06f7467caca..b3493007770 100644
--- a/src/main/target/STM32F411/target.h
+++ b/src/main/target/STM32F411/target.h
@@ -49,6 +49,8 @@
 #define USE_I2C
 #define I2C_FULL_RECONFIGURABILITY
 
+#define USE_DSHOT_BITBAND
+
 #define USE_BEEPER
 
 #ifdef USE_SDCARD
diff --git a/src/main/target/STM32H723/target.h b/src/main/target/STM32H723/target.h
index a60a97fd65d..47ae65e39a9 100644
--- a/src/main/target/STM32H723/target.h
+++ b/src/main/target/STM32H723/target.h
@@ -20,15 +20,35 @@
 
 #pragma once
 
+#ifndef TARGET_BOARD_IDENTIFIER
 #define TARGET_BOARD_IDENTIFIER "SH72"
+#endif
 
+#ifndef USBD_PRODUCT_STRING
 #define USBD_PRODUCT_STRING     "Betaflight STM32H723"
+#endif
 
+#if !defined(USE_I2C)
+#define USE_I2C
 #define USE_I2C_DEVICE_1
 #define USE_I2C_DEVICE_2
 #define USE_I2C_DEVICE_3
 #define USE_I2C_DEVICE_4
 #define USE_I2C_DEVICE_5
+#define I2C_FULL_RECONFIGURABILITY
+#endif
+
+// Provide a default so that this target builds on the build server.
+#if !defined(USE_SPI)
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define USE_SPI_DEVICE_2
+#define USE_SPI_DEVICE_3
+#define USE_SPI_DEVICE_4
+#define USE_SPI_DEVICE_5
+#define USE_SPI_DEVICE_6
+#define SPI_FULL_RECONFIGURABILITY
+#endif
 
 #define USE_UART1
 #define USE_UART2
@@ -43,13 +63,6 @@
 
 #define SERIAL_PORT_COUNT       (UNIFIED_SERIAL_PORT_COUNT + 10)
 
-#define USE_SPI_DEVICE_1
-#define USE_SPI_DEVICE_2
-#define USE_SPI_DEVICE_3
-#define USE_SPI_DEVICE_4
-#define USE_SPI_DEVICE_5
-#define USE_SPI_DEVICE_6
-
 #define TARGET_IO_PORTA 0xffff
 #define TARGET_IO_PORTB 0xffff
 #define TARGET_IO_PORTC 0xffff
@@ -58,9 +71,6 @@
 #define TARGET_IO_PORTF 0xffff
 #define TARGET_IO_PORTG 0xffff
 
-#define USE_I2C
-#define I2C_FULL_RECONFIGURABILITY
-
 #define USE_BEEPER
 
 #ifdef USE_SDCARD
@@ -68,9 +78,6 @@
 #define USE_SDCARD_SDIO
 #endif
 
-#define USE_SPI
-#define SPI_FULL_RECONFIGURABILITY
-
 #define USE_VCP
 
 #define USE_SOFTSERIAL1
@@ -84,4 +91,10 @@
 
 #define USE_ADC
 
+#if defined(USE_LED_STRIP) && !defined(USE_LEDSTRIP_CACHE_MGMT)
+#define USE_LEDSTRIP_CACHE_MGMT
+#endif
+
+#if !defined(USE_EXST)
 #define USE_CUSTOM_DEFAULTS
+#endif
diff --git a/src/main/target/STM32H723/target.mk b/src/main/target/STM32H723/target.mk
index 0930806cbaf..5fb733e16d6 100644
--- a/src/main/target/STM32H723/target.mk
+++ b/src/main/target/STM32H723/target.mk
@@ -19,7 +19,9 @@ RX_SRC = \
 
 H723xG_TARGETS += $(TARGET)
 
+ifneq ($(EXST),yes)
 CUSTOM_DEFAULTS_EXTENDED = yes
+endif
 
 FEATURES       += VCP SDCARD_SPI SDCARD_SDIO ONBOARDFLASH
 
diff --git a/src/main/target/STM32H730/target.h b/src/main/target/STM32H730/target.h
index 4f1404b8469..80f43e04ac7 100644
--- a/src/main/target/STM32H730/target.h
+++ b/src/main/target/STM32H730/target.h
@@ -74,7 +74,7 @@
 #define USE_UART8
 #define USE_UART9
 #define USE_UART10
-#define USE_LP_UART1
+#define USE_LPUART1
 
 #define SERIAL_PORT_COUNT       (UNIFIED_SERIAL_PORT_COUNT + 11)
 
@@ -103,6 +103,10 @@
 
 #define USE_ADC
 
+#if defined(USE_LED_STRIP) && !defined(USE_LEDSTRIP_CACHE_MGMT)
+#define USE_LEDSTRIP_CACHE_MGMT
+#endif
+
 // Provide a default so that this target builds on the build server.
 #if !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_SDCARD) && !defined(CONFIG_IN_EXTERNAL_FLASH)
 #define CONFIG_IN_RAM
diff --git a/src/main/target/STM32H750/target.h b/src/main/target/STM32H750/target.h
index 71b442528aa..bc74503aaaf 100644
--- a/src/main/target/STM32H750/target.h
+++ b/src/main/target/STM32H750/target.h
@@ -84,9 +84,9 @@
 #define USE_UART6
 #define USE_UART7
 #define USE_UART8
-#define USE_LP_UART1
+#define USE_LPUART1
 
-#define SERIAL_PORT_COUNT       (UNIFIED_SERIAL_PORT_COUNT + 8)
+#define SERIAL_PORT_COUNT       (UNIFIED_SERIAL_PORT_COUNT + 9)
 
 #define TARGET_IO_PORTA 0xffff
 #define TARGET_IO_PORTB 0xffff
@@ -110,6 +110,10 @@
 
 #define USE_ADC
 
+#if defined(USE_LED_STRIP) && !defined(USE_LEDSTRIP_CACHE_MGMT)
+#define USE_LEDSTRIP_CACHE_MGMT
+#endif
+
 // Provide a default so that this target builds on the build server.
 #if !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_SDCARD) && !defined(CONFIG_IN_EXTERNAL_FLASH)
 #define CONFIG_IN_RAM
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index 61049fccc2c..12502b6fcac 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -151,8 +151,17 @@
 #undef USE_TELEMETRY_SRXL
 
 #ifdef USE_SERIALRX_FPORT
+#ifndef USE_TELEMETRY
 #define USE_TELEMETRY
 #endif
+#ifndef USE_TELEMETRY_SMARTPORT
+#define USE_TELEMETRY_SMARTPORT
+#endif
+#endif
+#endif
+
+#if defined(USE_TELEMETRY_IBUS_EXTENDED) && !defined(USE_TELEMETRY_IBUS)
+#define USE_TELEMETRY_IBUS
 #endif
 
 #if !defined(USE_SERIALRX_CRSF)
@@ -233,6 +242,10 @@
 #undef USE_ADC_INTERNAL
 #endif
 
+#if (defined(USE_SDCARD) || defined(USE_FLASH)) && !defined(USE_BLACKBOX)
+#define USE_BLACKBOX
+#endif
+
 #ifdef USE_FLASH
 #define USE_FLASHFS
 #define USE_FLASH_TOOLS
@@ -254,6 +267,18 @@
 #define USE_FLASH_CHIP
 #endif
 
+#if defined(USE_SPI) && (defined(USE_FLASH_M25P16) || defined(USE_FLASH_W25M512) || defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25M02G))
+#define USE_FLASH_SPI
+#endif
+
+#if defined(USE_QUADSPI) && (defined(USE_FLASH_W25Q128FV) || defined(USE_FLASH_W25N01G))
+#define USE_FLASH_QUADSPI
+#endif
+
+#if defined(USE_OCTOSPI) && (defined(USE_FLASH_W25Q128FV))
+#define USE_FLASH_OCTOSPI
+#endif
+
 #ifndef USE_FLASH_CHIP
 #undef USE_FLASHFS
 #endif
@@ -306,6 +331,16 @@
 #define USE_SPI_GYRO
 #endif
 
+#ifndef SIMULATOR_BUILD
+#ifndef USE_ACC
+#define USE_ACC
+#endif
+
+#ifndef USE_GYRO
+#define USE_GYRO
+#endif
+#endif
+
 // CX10 is a special case of SPI RX which requires XN297
 #if defined(USE_RX_CX10)
 #define USE_RX_XN297
@@ -426,7 +461,7 @@
 #undef USE_ESCSERIAL
 #endif
 
-#if defined(CONFIG_IN_RAM) || defined(CONFIG_IN_FILE) || defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_SDCARD)
+#if defined(CONFIG_IN_RAM) || defined(CONFIG_IN_FILE) || defined(CONFIG_IN_EXTERNAL_FLASH) || defined(CONFIG_IN_SDCARD) || defined(CONFIG_IN_MEMORY_MAPPED_FLASH)
 #ifndef EEPROM_SIZE
 #define EEPROM_SIZE     4096
 #endif
@@ -445,6 +480,29 @@ extern uint8_t __config_end;
 #define USE_FLASH_BOOT_LOADER
 #endif
 
+#if defined(USE_FLASH_MEMORY_MAPPED)
+#if !defined(USE_RAM_CODE)
+#define USE_RAM_CODE
+#endif
+
+#define MMFLASH_CODE RAM_CODE
+#define MMFLASH_CODE_NOINLINE RAM_CODE NOINLINE
+
+#define MMFLASH_DATA FAST_DATA
+#define MMFLASH_DATA_ZERO_INIT FAST_DATA_ZERO_INIT
+#else
+#define MMFLASH_CODE
+#define MMFLASH_CODE_NOINLINE
+#define MMFLASH_DATA
+#define MMFLASH_DATA_ZERO_INIT
+#endif
+
+#ifdef USE_RAM_CODE
+// RAM_CODE for methods that need to be in RAM, but don't need to be in the fastest type of memory.
+// Note: if code is marked as RAM_CODE it *MUST* be in RAM, there is no alternative unlike functions marked with FAST_CODE/CCM_CODE
+#define RAM_CODE                   __attribute__((section(".ram_code")))
+#endif
+
 #if !defined(USE_RPM_FILTER)
 #undef USE_DYN_IDLE
 #endif
diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h
index ab2971733d8..801504944c5 100644
--- a/src/main/target/common_pre.h
+++ b/src/main/target/common_pre.h
@@ -421,7 +421,7 @@ extern uint8_t _dmaram_end__;
 #endif // !defined(CLOUD_BUILD)
 
 #if !defined(LED_MAX_STRIP_LENGTH)
-#ifdef USE_LEDSTRIP_64
+#ifdef USE_LED_STRIP_64
 #define LED_MAX_STRIP_LENGTH           64
 #else
 #define LED_MAX_STRIP_LENGTH           32
@@ -439,10 +439,6 @@ extern uint8_t _dmaram_end__;
 #endif
 #endif
 
-#if (defined(USE_SDCARD) || defined(USE_FLASH)) && !defined(USE_BLACKBOX)
-#define USE_BLACKBOX
-#endif
-
 #if defined(USE_PINIO)
 #define USE_PINIOBOX
 #define USE_PIN_PULL_UP_DOWN
diff --git a/src/test/Makefile b/src/test/Makefile
index 6d287cb274a..b3f942b41b6 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -477,12 +477,13 @@ GTEST_DIR = ../../lib/test/gtest
 
 CC  := clang-12
 CXX := clang++-12
+ifneq ($(OSFAMILY), linux)
 ifeq ($(shell which $(CC) 2>/dev/null),)
-$(info Falling back to 'clang'.)
+$(info Falling back to 'clang' on Windows and OSX.)
 CC  := clang
 CXX := clang++
 endif
-
+endif
 #CC  := gcc
 #CXX := g++
 
diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc
index 6c1a8d672c8..2fceb237249 100644
--- a/src/test/unit/arming_prevention_unittest.cc
+++ b/src/test/unit/arming_prevention_unittest.cc
@@ -97,8 +97,10 @@ extern "C" {
     bool mockIsUpright = false;
     uint8_t activePidLoopDenom = 1;
 
-    float gpsGetSampleRateHz(void) { return 10.0f; }
+    float getGpsDataIntervalSeconds(void) { return 0.1f; }
+    void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k) { filter->k = k; }
     void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) { filter->k = k; }
+    void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k) { filter->k = k; }
 }
 
 uint32_t simulationFeatureFlags = 0;
@@ -1143,11 +1145,18 @@ extern "C" {
         UNUSED(throttleDLpf);
         return 0.0f;
     }
-    void pt3FilterInit(pt3Filter_t *pitchLpf, float) {
-        UNUSED(pitchLpf);
+    void pt1FilterInit(pt1Filter_t *velocityDLpf, float) {
+        UNUSED(velocityDLpf);
     }
-    float pt3FilterApply(pt3Filter_t *pitchLpf, float) {
-        UNUSED(pitchLpf);
+    float pt1FilterApply(pt1Filter_t *velocityDLpf, float) {
+        UNUSED(velocityDLpf);
+        return 0.0f;
+    }
+    void pt3FilterInit(pt3Filter_t *velocityUpsampleLpf, float) {
+        UNUSED(velocityUpsampleLpf);
+    }
+    float pt3FilterApply(pt3Filter_t *velocityUpsampleLpf, float) {
+        UNUSED(velocityUpsampleLpf);
         return 0.0f;
     }
     void getRcDeflectionAbs(void) {}
diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc
index 3810dae587f..a36307cac18 100644
--- a/src/test/unit/cli_unittest.cc
+++ b/src/test/unit/cli_unittest.cc
@@ -66,7 +66,8 @@ extern "C" {
     const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);
     const lookupTableEntry_t lookupTables[] = {};
     const char * const lookupTableOsdDisplayPortDevice[] = {};
-
+    const char * const buildKey = NULL;
+    const char * const releaseName = NULL;
 
     PG_REGISTER(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0);
     PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc
index c901df0fb09..2ec9304f47d 100644
--- a/src/test/unit/flight_imu_unittest.cc
+++ b/src/test/unit/flight_imu_unittest.cc
@@ -213,7 +213,6 @@ extern "C" {
     mag_t mag;
 
     gpsSolutionData_t gpsSol;
-    int16_t GPS_verticalSpeedInCmS;
 
     uint8_t debugMode;
     int16_t debug[DEBUG16_VALUE_COUNT];
@@ -257,6 +256,7 @@ extern "C" {
     float baroUpsampleAltitude()  { return 0.0f; }
     float pt2FilterGain(float, float)  { return 0.0f; }
     float getBaroAltitude(void) { return 3000.0f; }
+    float gpsRescueGetImuYawCogGain(void) { return 1.0f; }
 
     void pt2FilterInit(pt2Filter_t *baroDerivativeLpf, float) {
         UNUSED(baroDerivativeLpf);
diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h
index 2a015243107..72355cf6c90 100644
--- a/src/test/unit/platform.h
+++ b/src/test/unit/platform.h
@@ -44,7 +44,7 @@
 #define USE_TRANSPONDER
 
 #ifndef LED_MAX_STRIP_LENGTH
-    #ifdef USE_LEDSTRIP_64
+    #ifdef USE_LED_STRIP_64
         #define LED_MAX_STRIP_LENGTH           64
     #else
         #define LED_MAX_STRIP_LENGTH           32
diff --git a/support/scripts/build_nucleoh723zg_mmapped.sh b/support/scripts/build_nucleoh723zg_mmapped.sh
new file mode 100644
index 00000000000..35cc3088b7b
--- /dev/null
+++ b/support/scripts/build_nucleoh723zg_mmapped.sh
@@ -0,0 +1,108 @@
+#!/bin/sh
+set -x
+
+make DEBUG=INFO EXST=yes EXST_ADJUST_VMA=0x90100000 TARGET=STM32H723 EXTRA_FLAGS="\
+-D'TARGET_BOARD_IDENTIFIER=\"H723\"' \
+-D'USBD_PRODUCT_STRING=\"Nucleo-H723-MMAPPED\"' \
+\
+-D'EEPROM_SIZE=8192' \
+\
+-DUSE_BUTTONS \
+-D'BUTTON_A_PIN=PC13' \
+-DBUTTON_A_PIN_INVERTED \
+-D'BUTTON_B_PIN=PC13' \
+-DBUTTON_B_PIN_INVERTED \
+\
+-DUSE_OCTOSPI \
+-DUSE_OCTOSPI_DEVICE_1 \
+-D'OCTOSPIM_P1_SCK_PIN=PB2' \
+-D'OCTOSPIM_P1_CS_PIN=PB6' \
+-D'OCTOSPIM_P1_IO0_PIN=PD11' \
+-D'OCTOSPIM_P1_IO1_PIN=PD12' \
+-D'OCTOSPIM_P1_IO2_PIN=PE2' \
+-D'OCTOSPIM_P1_IO3_PIN=PD13' \
+-D'OCTOSPIM_P1_IO4_PIN=NONE' \
+-D'OCTOSPIM_P1_IO5_PIN=NONE' \
+-D'OCTOSPIM_P1_IO6_PIN=NONE' \
+-D'OCTOSPIM_P1_IO7_PIN=NONE' \
+\
+-D'OCTOSPIM_P1_MODE=OCTOSPIM_P1_MODE_IO03_ONLY' \
+-D'OCTOSPIM_P1_CS_FLAGS=(OCTOSPIM_P1_CS_HARDWARE)' \
+\
+-DUSE_SPI \
+\
+-DUSE_SPI_DEVICE_1 \
+-D'SPI1_SCK_PIN=PB3' \
+-D'SPI1_MISO_PIN=PB4' \
+-D'SPI1_MOSI_PIN=PB5' \
+\
+-DUSE_SPI_DEVICE_2 \
+-D'SPI2_SCK_PIN=NONE' \
+-D'SPI2_MISO_PIN=NONE' \
+-D'SPI2_MOSI_PIN=NONE' \
+\
+-DUSE_SPI_DEVICE_3 \
+-D'SPI3_SCK_PIN=PC10' \
+-D'SPI3_MISO_PIN=PC11' \
+-D'SPI3_MOSI_PIN=PC12' \
+\
+-DUSE_SPI_DEVICE_4 \
+-D'SPI4_SCK_PIN=NONE' \
+-D'SPI4_MISO_PIN=NONE' \
+-D'SPI4_MOSI_PIN=NONE' \
+\
+-DUSE_SPI_DEVICE_5 \
+-D'SPI5_SCK_PIN=NONE' \
+-D'SPI5_MISO_PIN=NONE' \
+-D'SPI5_MOSI_PIN=NONE' \
+\
+-DUSE_SPI_DEVICE_6 \
+-D'SPI6_SCK_PIN=NONE' \
+-D'SPI6_MISO_PIN=NONE' \
+-D'SPI6_MOSI_PIN=NONE' \
+\
+-DUSE_FLASH_TOOLS \
+-DUSE_FLASH_W25Q128FV \
+-DUSE_FLASH_MEMORY_MAPPED \
+-DCONFIG_IN_MEMORY_MAPPED_FLASH \
+-D'FLASH_SPI_INSTANCE=NULL' \
+-D'FLASH_CS_PINS=NONE' \
+\
+-DUSE_SDCARD \
+-D'SDCARD_DETECT_PIN=NONE' \
+-D'SDIO_DEVICE=SDIODEV_2' \
+-D'SDIO_USE_4BIT=false' \
+-D'SDIO_CK_PIN=PD6' \
+-D'SDIO_CMD_PIN=PD7' \
+-D'SDIO_D0_PIN=PB14' \
+-D'SDIO_D1_PIN=NONE' \
+-D'SDIO_D2_PIN=NONE' \
+-D'SDIO_D3_PIN=NONE' \
+\
+-D'TARGET_IO_PORTA=0xffff' \
+-D'TARGET_IO_PORTB=0xffff' \
+-D'TARGET_IO_PORTC=0xffff' \
+-D'TARGET_IO_PORTD=0xffff' \
+-D'TARGET_IO_PORTE=0xffff' \
+-D'TARGET_IO_PORTF=0xffff' \
+-D'TARGET_IO_PORTG=0xffff' \
+\
+-DUSE_USB_ID \
+\
+-DUSE_I2C \
+-DUSE_I2C_DEVICE_1 \
+-D'I2C1_SCL=PB8' \
+-D'I2C1_SDA=PB9' \
+-D'I2C_DEVICE=(I2CDEV_1)' \
+\
+-DUSE_ACC \
+-DUSE_ACC_SPI_MPU6500 \
+-DUSE_GYRO \
+-DUSE_GYRO_SPI_MPU6500 \
+\
+-D'ADC1_DMA_OPT=8' \
+-D'ADC2_DMA_OPT=9' \
+-D'ADC3_DMA_OPT=10' \
+\
+-DENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT \
+"
\ No newline at end of file
diff --git a/support/scripts/build_spracingh7rf.sh b/support/scripts/build_spracingh7rf.sh
new file mode 100644
index 00000000000..9f51863e542
--- /dev/null
+++ b/support/scripts/build_spracingh7rf.sh
@@ -0,0 +1,145 @@
+#!/bin/sh
+set -x
+
+make DEBUG=INFO TARGET=STM32H730 EXTRA_FLAGS="\
+-D'TARGET_BOARD_IDENTIFIER=\"SP7R\"' \
+-D'USBD_PRODUCT_STRING=\"SPRacingH7RF\"' \
+\
+-D'EEPROM_SIZE=8192' \
+-DUSE_SPRACING_PERSISTENT_RTC_WORKAROUND \
+\
+-DUSE_BUTTONS  \
+-D'BUTTON_A_PIN=PC14' \
+-DBUTTON_A_PIN_INVERTED \
+-D'BUTTON_B_PIN=PC14' \
+-DBUTTON_B_PIN_INVERTED \
+\
+-DUSE_OCTOSPI \
+-DUSE_OCTOSPI_DEVICE_1 \
+-D'OCTOSPIM_P1_SCK_PIN=PB2' \
+-D'OCTOSPIM_P1_CS_PIN=PB10' \
+-D'OCTOSPIM_P1_IO0_PIN=PD11' \
+-D'OCTOSPIM_P1_IO1_PIN=PD12' \
+-D'OCTOSPIM_P1_IO2_PIN=PE2' \
+-D'OCTOSPIM_P1_IO3_PIN=PD13' \
+-D'OCTOSPIM_P1_IO4_PIN=NONE' \
+-D'OCTOSPIM_P1_IO5_PIN=NONE' \
+-D'OCTOSPIM_P1_IO6_PIN=NONE' \
+-D'OCTOSPIM_P1_IO7_PIN=NONE' \
+-D'OCTOSPIM_P1_MODE=OCTOSPIM_P1_MODE_IO03_ONLY' \
+-D'OCTOSPIM_P1_CS_FLAGS=(OCTOSPIM_P1_CS_HARDWARE)' \
+\
+-DUSE_SPI \
+\
+-DUSE_SPI_DEVICE_2 \
+-D'SPI2_SCK_PIN=PD3' \
+-D'SPI2_MISO_PIN=PB14' \
+-D'SPI2_MOSI_PIN=PB15' \
+-D'SPI2_NSS_PIN=PB12' \
+\
+-DUSE_SPI_DEVICE_6 \
+-D'SPI6_SCK_PIN=PB3' \
+-D'SPI6_MISO_PIN=PB4' \
+-D'SPI6_MOSI_PIN=PB5' \
+-D'SPI6_NSS_PIN=PA15' \
+\
+-D'SX1280_BUSY_PIN=PC7' \
+-D'SX1280_DIO1_PIN=PC6' \
+-D'SX1280_DIO2_PIN=PD4' \
+-D'SX1280_DIO3_PIN=NONE' \
+-D'SX1280_NRESET_PIN=PD10' \
+-DUSE_RX_SPI \
+-DUSE_RX_EXPRESSLRS \
+-DUSE_RX_SX1280 \
+-D'RX_SPI_INSTANCE=SPI2' \
+-D'RX_NSS_PIN=SPI2_NSS_PIN' \
+-D'RX_SPI_EXTI_PIN=SX1280_DIO1_PIN' \
+-D'RX_EXPRESSLRS_SPI_RESET_PIN=SX1280_NRESET_PIN' \
+-D'RX_EXPRESSLRS_SPI_BUSY_PIN=SX1280_BUSY_PIN' \
+-D'RX_EXPRESSLRS_TIMER_INSTANCE=TIM6' \
+-D'DEFAULT_RX_FEATURE=FEATURE_RX_SPI' \
+-D'RX_SPI_DEFAULT_PROTOCOL=RX_SPI_EXPRESSLRS' \
+\
+-D'VTX_ENABLE_PIN=PC15' \
+-D'PINIO1_PIN=VTX_ENABLE_PIN' \
+\
+-DUSE_FLASH_MEMORY_MAPPED \
+-DUSE_FLASH_W25Q128FV \
+-D'FLASH_OCTOSPI_INSTANCE=OCTOSPI1' \
+-DCONFIG_IN_MEMORY_MAPPED_FLASH \
+-DUSE_FIRMWARE_PARTITION \
+\
+-D'SDCARD_DETECT_PIN=PC13' \
+-DSDCARD_DETECT_INVERTED \
+-D'SDIO_DEVICE=SDIODEV_1' \
+-D'SDIO_USE_4BIT=true' \
+-D'SDIO_CK_PIN=PC12' \
+-D'SDIO_CMD_PIN=PD2' \
+-D'SDIO_D0_PIN=PC8' \
+-D'SDIO_D1_PIN=PC9' \
+-D'SDIO_D2_PIN=PC10' \
+-D'SDIO_D3_PIN=PC11' \
+\
+-D'TARGET_IO_PORTA=0xffff' \
+-D'TARGET_IO_PORTB=(0xffff & ~(BIT(2)|BIT(6)))' \
+-D'TARGET_IO_PORTC=0xffff' \
+-D'TARGET_IO_PORTD=(0xffff & ~(BIT(11)|BIT(12)|BIT(13)))' \
+-D'TARGET_IO_PORTE=(0xffff & ~(BIT(2)|BIT(7)|BIT(8)|BIT(9)|BIT(10)))' \
+-D'TARGET_IO_PORTF=0xffff' \
+-D'TARGET_IO_PORTG=0xffff' \
+-D'TARGET_IO_PORTH=0xffff' \
+\
+-DUSE_I2C \
+-DUSE_I2C_DEVICE_1 \
+-D'I2C1_SCL=PB8' \
+-D'I2C1_SDA=PB9' \
+\
+-DUSE_I2C_DEVICE_2 \
+-D'I2C2_SCL=PB10' \
+-D'I2C2_SDA=PB11' \
+-D'MAG_I2C_INSTANCE=(I2CDEV_1)' \
+-D'BARO_I2C_INSTANCE=(I2CDEV_2)' \
+\
+-DUSE_ACC \
+-DUSE_GYRO \
+\
+-DUSE_MPU_DATA_READY_SIGNAL \
+-DENSURE_MPU_DATA_READY_IS_LOW \
+\
+-D'ADC3_DMA_OPT=10' \
+-D'ADC_INSTANCE=ADC3' \
+-D'CURRENT_METER_2_ADC_PIN=PC0' \
+-D'CURRENT_METER_2_ADC_INSTANCE=ADC3' \
+-D'CURRENT_METER_1_ADC_PIN=PC1' \
+-D'CURRENT_METER_1_ADC_INSTANCE=ADC3' \
+-D'EXTERNAL1_ADC_PIN=PC2' \
+-D'EXTERNAL1_ADC_INSTANCE=ADC3' \
+-D'VIDEO_IN_ADC_PIN=PC5' \
+-D'VIDEO_OUT_ADC_PIN=PC4' \
+-D'VBAT_ADC_PIN=PC3' \
+-D'VBAT_ADC_INSTANCE=ADC3' \
+-D'RSSI_ADC_PIN=CURRENT_METER_2_ADC_PIN' \
+-D'RSSI_ADC_INSTANCE=CURRENT_METER_2_ADC_INSTANCE' \
+-D'CURRENT_METER_ADC_PIN=CURRENT_METER_1_ADC_PIN' \
+-D'CURRENT_METER_ADC_INSTANCE=CURRENT_METER_1_ADC_INSTANCE' \
+-D'DEFAULT_VOLTAGE_METER_SOURCE=VOLTAGE_METER_ADC' \
+-D'DEFAULT_CURRENT_METER_SOURCE=CURRENT_METER_ADC' \
+\
+-DENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT \
+\
+-DUSE_SDCARD \
+-DUSE_ACC_SPI_ICM42605 \
+-DUSE_ACC_SPI_ICM42688P \
+-DUSE_GYRO_SPI_ICM42605 \
+-DUSE_GYRO_SPI_ICM42688P \
+-DUSE_FLASH_W25Q128FV \
+"
+
+# Settings that are currently defined in target/common_pre.h for non-cloud builds that probably shouldn't be.
+# There are here to illustrate that they SHOULD be included in THIS target when they are removed by default.
+
+#-DUSE_MAG \
+#-DUSE_MAG_HMC5883 \
+#-DUSE_MAG_QMC5883 \
+#-DUSE_BARO \
+#-DUSE_BARO_BMP388 \