[Mission]: Update the mission mode to use the planned mission interface library

[Mission] Add planned mission interface to easily extract data from dataman.
Commander params: improve meta data for max flight time and wind
2026-06-15 00:50:05 +08:00 · 2023-02-01 09:03:33 +01:00 · 2023-02-01 09:03:33 +01:00 · 2023-02-01 08:48:09 +01:00 · 2023-02-01 08:48:09 +01:00 · 2023-02-01 08:48:09 +01:00
164 changed files with 8265 additions and 7136 deletions
@@ -769,7 +769,13 @@ void runTests() {

  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params/parameters.bson"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd readtest"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd rwtest"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd erase"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params" || true' // expected to fail after erase

  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"'
@@ -73,6 +73,13 @@ menu "Toolchain"
        help
            relative path to the ROMFS root directory

+    config BOARD_ROOTFSDIR
+        string "Root directory"
+        depends on PLATFORM_POSIX
+        default "."
+        help
+            Configure the root directory in the file system for PX4 files
+
    config BOARD_IO
        string "IO board name"
        default "px4_io-v2_default"
@@ -112,21 +112,7 @@ else
 	#
 	if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params
 	then
-		set PARAM_FILE /fs/mtd_params/parameters.bson
-	else
-		# Revert after the transition period is completed
-		set PARAM_FILE /fs/mtd_params/parameters.bson
-		param select $PARAM_FILE
-		echo "Start parameter transition to LittleFS"
-		if ! param transition
-		then
-			echo "ERROR [init] param transition"
-		else
-			echo "The system will reboot!"
-			# Wait for UART to send messages
-			usleep 200000
-			reboot
-		fi
+		set PARAM_FILE /fs/mtd_params
 	fi

 	#
@@ -48,7 +48,7 @@ do
 	${DIR}/reboot.py --device ${SERIAL_DEVICE}

 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param status'
-	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'bsondump /fs/mtd_params/parameters.bson' || true
+	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'bsondump /fs/mtd_params' || true
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'bsondump /fs/microsd/parameters_backup.bson' || true

 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'ps'
@@ -94,7 +94,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -154,7 +153,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -74,7 +74,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -129,7 +128,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -74,7 +74,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -131,7 +130,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -74,7 +74,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -131,7 +130,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -105,11 +105,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -194,8 +189,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -34,7 +34,7 @@
 #include <nuttx/spi/spi.h>
 #include <px4_platform_common/px4_manifest.h>
 //                                                              KiB BS    nB
-static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 1 x 32768
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 512 X 64
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -45,12 +45,18 @@ static const px4_mft_device_t i2c3 = {             // 24LC64T on Base  8K 32 X 2

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi5,
-	.npart = 1
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32768
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -99,7 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -163,7 +162,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -107,7 +107,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -192,7 +191,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -94,7 +94,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -151,7 +150,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -42,12 +42,18 @@ static const px4_mft_device_t i2c1 = {             // 24AA64FT on Base  8K 32 X

 static const px4_mtd_entry_t fmu_eeprom = {
 	.device = &i2c1,
-	.npart = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 256
+			.nblocks = 128
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 128
+
 		}
 	},
 };
@@ -151,7 +151,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -76,7 +76,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -133,7 +132,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -98,7 +98,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -163,7 +162,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -99,11 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -152,8 +147,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -169,7 +162,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -99,11 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=1
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=16
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -48,7 +48,6 @@ else()
 		i2c.cpp
 		init.c
 		led.c
-		mtd.cpp
 		spi.cpp
 		timer_config.cpp
 		usb.c
@@ -99,11 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -153,8 +148,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -170,7 +163,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -100,11 +100,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -153,8 +148,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -170,7 +163,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -99,7 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -163,7 +162,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -88,7 +88,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -148,7 +147,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -77,7 +77,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -134,7 +133,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -100,7 +100,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -165,7 +164,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -163,7 +163,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -99,7 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -163,7 +162,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -100,7 +100,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -159,7 +158,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -98,7 +98,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -162,7 +161,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -100,7 +100,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -168,7 +167,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -41,12 +41,18 @@ static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 512 X

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi5,
-	.npart = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32768
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -1,9 +1,9 @@
 CONFIG_PLATFORM_QURT=y
 CONFIG_BOARD_TOOLCHAIN="qurt"
 CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
+CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
-CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -414,7 +414,7 @@ bool ICM42688P::Configure()
 	return success;
 }

-static bool interrupt_debug = true;
+static bool interrupt_debug = false;
 static uint32_t interrupt_debug_count = 0;
 static const uint32_t interrupt_debug_trigger = 800;
 static hrt_abstime last_interrupt_time = 0;
@@ -17,3 +17,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_ORB_COMMUNICATOR=y
+CONFIG_BOARD_ROOTFSDIR="/data/px4"
@@ -19,5 +19,3 @@ fi
 muorb start

 qshell icm42688p start -s
-
-qshell modal_io start
@@ -98,7 +98,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -162,7 +161,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -98,7 +98,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -162,7 +161,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -98,7 +98,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -163,7 +162,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -98,7 +98,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -163,7 +162,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -99,7 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -164,7 +163,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -98,7 +98,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -162,7 +161,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -95,7 +95,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -155,7 +154,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -51,7 +51,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
@@ -181,7 +180,7 @@ CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
@@ -52,7 +52,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
@@ -184,7 +183,7 @@ CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
@@ -55,7 +55,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
@@ -184,7 +183,7 @@ CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
@@ -53,7 +53,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
@@ -183,7 +182,7 @@ CONFIG_SCHED_INSTRUMENTATION=y
 CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
@@ -53,11 +53,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
@@ -176,8 +171,6 @@ CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x1fff0000
@@ -58,7 +58,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
@@ -200,7 +199,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -122,7 +122,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
 CONFIG_SERIAL_TERMIOS=y
@@ -111,7 +111,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -94,7 +94,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_ROMFS=y
 CONFIG_GRAN=y
 CONFIG_GRAN_INTR=y
@@ -150,7 +149,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -96,7 +96,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -155,7 +154,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -95,11 +95,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -145,8 +140,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -160,7 +153,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -96,11 +96,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -146,8 +141,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -95,11 +95,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -145,8 +140,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=393216
 CONFIG_RAM_START=0x20000000
@@ -162,7 +155,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -96,11 +96,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -146,8 +141,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=393216
 CONFIG_RAM_START=0x20000000
@@ -46,7 +46,7 @@ static const px4_mtd_entry_t fmum_fram = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 16384
+			.nblocks = 32
 		},
 	},
 };
@@ -100,11 +100,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -132,8 +127,6 @@ CONFIG_MM_REGIONS=3
 CONFIG_MTD=y
 CONFIG_MTD_BYTE_WRITE=y
 CONFIG_MTD_PARTITION=y
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
 CONFIG_NSH_ARCHINIT=y
@@ -170,7 +163,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -100,11 +100,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -153,8 +148,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -170,7 +163,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -144,11 +144,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -197,8 +192,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -99,11 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -151,8 +146,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -168,7 +161,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -102,11 +102,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -157,8 +152,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -99,11 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -152,8 +147,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -169,7 +162,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SDMMC1_SDIO_PULLUP=y
@@ -99,11 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -151,8 +146,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -104,11 +104,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -190,8 +185,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -207,7 +200,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -103,11 +103,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -188,8 +183,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -34,7 +34,7 @@
 #include <nuttx/spi/spi.h>
 #include <px4_platform_common/px4_manifest.h>
 //                                                              KiB BS    nB
-static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 1 X 32768
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 512 X 64
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -50,12 +50,18 @@ static const px4_mft_device_t i2c4 = {             // 24LC64T on IMU   8K 32 X 2

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi5,
-	.npart = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32768
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -99,11 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -156,8 +151,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -34,7 +34,7 @@
 #include <nuttx/spi/spi.h>
 #include <px4_platform_common/px4_manifest.h>
 //                                                              KiB BS    nB
-static const px4_mft_device_t spi2 = {             // FM25V02A on FMUM 32K 1 x 32768
+static const px4_mft_device_t spi2 = {             // FM25V02A on FMUM 32K 512 X 64
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -47,12 +47,18 @@ static const px4_mft_device_t i2c4 = {             // 24LC64T on IMU   8K 32 X 2

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi2,
-	.npart = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32768
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -99,11 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -156,8 +151,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -173,7 +166,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -34,19 +34,25 @@
 #include <nuttx/spi/spi.h>
 #include <px4_platform_common/px4_manifest.h>
 //                                                              KiB BS    nB
-static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 1 x 32768
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 512 X 64
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi5,
-	.npart = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32768
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -105,11 +105,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
-CONFIG_FS_LITTLEFS_BLOCK_CYCLE=-1
-CONFIG_FS_LITTLEFS_BLOCK_SIZE_FACTOR=128
-CONFIG_FS_LITTLEFS_LOOKAHEAD_SIZE=8
-CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=1
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -194,8 +189,6 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
-CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=0
-CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=0
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -211,7 +204,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC2_SDIO_PULLUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -34,7 +34,7 @@
 #include <nuttx/spi/spi.h>
 #include <px4_platform_common/px4_manifest.h>
 //                                                              KiB BS    nB
-static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 1 x 32768
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM 32K 512 X 64
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -50,12 +50,18 @@ static const px4_mft_device_t i2c4 = {             // 24LC64T on IMU   8K 32 X 2

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi5,
-	.npart = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32768
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -39,7 +39,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_REGISTER=y
 CONFIG_FS_ROMFS=y
@@ -91,7 +90,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_TERMIOS=y
@@ -99,7 +99,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
@@ -156,7 +155,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDMMC1_SDIO_MODE=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -94,7 +94,6 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
-CONFIG_FS_LITTLEFS=y
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
 CONFIG_FS_PROCFS_REGISTER=y
@@ -154,7 +153,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
 CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
-CONFIG_SCHED_LPWORKSTACKSIZE=1932
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
 CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
@@ -31,7 +31,7 @@ bool height_sensor_timeout              #  4 - true when the height sensor has n
 bool stopping_navigation                #  5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
 bool invalid_accel_bias_cov_reset       #  6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements
 bool bad_yaw_using_gps_course           #  7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
-bool stopping_mag_use                   #  8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
+bool stopping_mag_use                   #  8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
 bool vision_data_stopped                #  9 - true when the vision system data has stopped for a significant time period
-bool emergency_yaw_reset_mag_stopped    # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
+bool emergency_yaw_reset_mag_stopped    # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
 bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
@@ -35,7 +35,7 @@ uint8 CS_EV_HGT = 14		# 14 - true when height data from external vision measurem
 uint8 CS_BETA = 15		# 15 - true when synthetic sideslip measurements are being fused
 uint8 CS_MAG_FIELD = 16		# 16 - true when only the magnetic field states are updated by the magnetometer
 uint8 CS_FIXED_WING = 17	# 17 - true when thought to be operating as a fixed wing vehicle with constrained sideslip
-uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetomer has been declared faulty and is no longer being used
+uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetometer has been declared faulty and is no longer being used
 uint8 CS_ASPD = 19		# 19 - true when airspeed measurements are being fused
 uint8 CS_GND_EFFECT = 20	# 20 - true when when protection from ground effect induced static pressure rise is active
 uint8 CS_RNG_STUCK = 21		# 21 - true when a stuck range finder sensor has been detected
@@ -15,6 +15,7 @@ uint32 mode_req_global_position
 uint32 mode_req_mission
 uint32 mode_req_offboard_signal
 uint32 mode_req_home_position
+uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded
 uint32 mode_req_prevent_arming    # if set, cannot arm while in this mode
 uint32 mode_req_other             # other requirements, not covered above (for external modes)

@@ -40,6 +40,8 @@
 #pragma once

 #include <sys/ioctl.h>
+#include <px4_boardconfig.h>
+

 /****************************************************************************
 * Defines for all platforms.
@@ -95,7 +97,7 @@ __BEGIN_DECLS
 extern long PX4_TICKS_PER_SEC;
 __END_DECLS

-#define PX4_ROOTFSDIR "."
+#define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR

 #define PX4_STORAGEDIR PX4_ROOTFSDIR

@@ -77,24 +77,6 @@ __EXPORT int  px4_mtd_get_geometry(const mtd_instance_s *instance, unsigned long
 */
 __EXPORT ssize_t px4_mtd_get_partition_size(const mtd_instance_s *instance, const char *partname);

-/*
-  Helper function for transition to LittleFS.
-  It will unmount MTD for parameters with LittleFS and mount Block Device
-*/
-__EXPORT int px4_mtd_unmount_littlefs_mount_block_device(void);
-
-/*
-  Helper function for transition to LittleFS.
-  It will unmount MTD for parameters with Block Device and mount LittleFS with force formatting
-*/
-__EXPORT int px4_mtd_unmount_block_device_mount_littlefs(void);
-
-/*
-  Helper function for transition to LittleFS.
-  It will force formatting to LittleFS
-*/
-__EXPORT int px4_mtd_forceformat_littlefs(void);
-
 int px4_at24c_initialize(FAR struct i2c_master_s *dev,
 			 uint8_t address, FAR struct mtd_dev_s **mtd_dev);

@@ -470,7 +470,7 @@ if(NOT NUTTX_DIR MATCHES "external")
 		)

 		# JLINK_RTOS_PATH used by launch.json.in
-		set(JLINK_RTOS_PATH ${NUTTX_DIR}/tools/jlink-nuttx)
+		set(JLINK_RTOS_PATH ${NUTTX_DIR}/tools/jlink-nuttx.so)
 	else()
 		set(JLINK_RTOS_PATH "")
 	endif()
@@ -3,6 +3,7 @@
    "configurations": [
        {
            "name": "jlink (@PX4_BOARD@)",
+            "gdbPath": "@CMAKE_GDB@",
            "device": "@DEBUG_DEVICE@",
            "svdFile": "@DEBUG_SVD_FILE_PATH@",
            "rtos": "@JLINK_RTOS_PATH@",
@@ -24,6 +25,7 @@
        },
        {
            "name": "stlink (@PX4_BOARD@)",
+            "gdbPath": "@CMAKE_GDB@",
            "device": "@DEBUG_DEVICE@",
            "svdFile": "@DEBUG_SVD_FILE_PATH@",
            "executable": "${command:cmake.launchTargetPath}",
@@ -42,6 +44,7 @@
        },
        {
            "name": "blackmagic (@PX4_BOARD@)",
+            "gdbPath": "@CMAKE_GDB@",
            "device": "@DEBUG_DEVICE@",
            "svdFile": "@DEBUG_SVD_FILE_PATH@",
            "executable": "${command:cmake.launchTargetPath}",
@@ -76,12 +76,9 @@ __EXPORT int px4_mft_configure(const px4_mft_s *mft)
 	if (mft != nullptr) {
 		for (uint32_t m = 0; m < mft->nmft; m++) {
 			switch (mft->mfts[m]->type) {
-#if defined(CONFIG_MTD)
-
 			case MTD:
 				px4_mtd_config(static_cast<const px4_mtd_manifest_t *>(mft->mfts[m]->pmft));
 				break;
-#endif // CONFIG_MTD

 			case MFT:
 			default:
@@ -102,12 +99,9 @@ __EXPORT int px4_mft_query(const px4_mft_s *mft, px4_manifest_types_e type,
 		for (uint32_t m = 0; m < mft->nmft; m++) {
 			if (mft->mfts[m]->type == type)
 				switch (type) {
-#if defined(CONFIG_MTD)
-
 				case MTD:
 					return px4_mtd_query(sub, val, nullptr);
 					break;
-#endif // CONFIG_MTD

 				case MFT:
 				default:
@@ -39,8 +39,6 @@
 * @author David Sidrane <david.sidrane@nscdg.com>
 */

-#if defined(CONFIG_MTD)
-
 #ifndef MODULE_NAME
 #define MODULE_NAME "PX4_MTD"
 #endif
@@ -59,7 +57,6 @@
 #include <nuttx/drivers/drivers.h>
 #include <nuttx/spi/spi.h>
 #include <nuttx/mtd/mtd.h>
-#include <nuttx/fs/fs.h>

 extern "C" {
 	struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
@@ -70,9 +67,6 @@ static int num_instances = 0;
 static int total_blocks = 0;
 static mtd_instance_s *instances[MAX_MTD_INSTANCES] = {};

-static int8_t param_instance = -1;
-static int8_t param_part = -1;
-static int8_t param_block = -1;

 static int ramtron_attach(mtd_instance_s &instance)
 {
@@ -124,6 +118,15 @@ static int ramtron_attach(mtd_instance_s &instance)
 		return -EIO;
 	}

+	int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, (unsigned long)spi_speed_mhz * 1000 * 1000);
+
+	if (ret != OK) {
+		// FIXME: From the previous warning call, it looked like this should have been fatal error instead. Tried
+		// that but setting the bus speed does fail all the time. Which was then exiting and the board would
+		// not run correctly. So changed to PX4_WARN.
+		PX4_WARN("failed to set bus speed");
+	}
+
 	return 0;
 #endif
 }
@@ -246,19 +249,25 @@ static const px4_mtd_manifest_t default_mtd_config = {

 #else

-const px4_mft_device_t spifram  = {             // FM25V02A on FMUM 32K 1 X 32768
+const px4_mft_device_t spifram  = {             // FM25V02A on FMUM 32K 512 X 64
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };

 const px4_mtd_entry_t fram = {
 	.device = &spifram,
-	.npart = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32768
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -392,54 +401,27 @@ memoryout:
 			}

 			/* Initialize to provide an FTL block driver on the MTD FLASH interface */
+
 			snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", total_blocks);

-			printf("blockname: %s, type: %d, name: %s\n", blockname, instances[i]->partition_types[part],
-			       instances[i]->partition_names[part]);
+			rv = ftl_initialize(total_blocks, instances[i]->part_dev[part]);

-			if (instances[i]->partition_types[part] == MTD_PARAMETERS) {
-
-				param_instance = i;
-				param_part = part;
-				param_block = total_blocks;
-
-				rv = register_mtddriver(blockname, instances[i]->part_dev[part], 0755, nullptr);
-
-				if (rv < 0) {
-					PX4_ERR("register_mtddriver %s failed: %d", blockname, rv);
-					goto errout;
-				}
-
-				// Now create a character device on the block device
-				//TODO: after the transition period return "autoformat"
-				rv = nx_mount(blockname, instances[i]->partition_names[part], "littlefs", 0, "");
-
-				printf("nx_mount: blockname: %s partition: %s\n", blockname, instances[i]->partition_names[part]);
-
-				if (rv < 0) {
-					PX4_ERR("nx_mount %s failed: %d", instances[i]->partition_names[part], rv);
-					goto errout;
-				}
-
-			} else {
-				rv = ftl_initialize(total_blocks, instances[i]->part_dev[part]);
-
-				if (rv < 0) {
-					PX4_ERR("ftl_initialize %s failed: %d", blockname, rv);
-					goto errout;
-				}
-
-				/* Now create a character device on the block device */
-				rv = bchdev_register(blockname, instances[i]->partition_names[part], false);
-
-				if (rv < 0) {
-					PX4_ERR("bchdev_register %s failed: %d", instances[i]->partition_names[part], rv);
-					goto errout;
-				}
+			if (rv < 0) {
+				PX4_ERR("ftl_initialize %s failed: %d", blockname, rv);
+				goto errout;
 			}

 			total_blocks++;

+			/* Now create a character device on the block device */
+
+			rv = bchdev_register(blockname, instances[i]->partition_names[part], false);
+
+			if (rv < 0) {
+				PX4_ERR("bchdev_register %s failed: %d", instances[i]->partition_names[part], rv);
+				goto errout;
+			}
+
 			instances[i]->n_partitions_current++;
 		}

@@ -500,88 +482,3 @@ __EXPORT int px4_mtd_query(const char *sub, const char *val, const char **get)

 	return rv;
 }
-
-int px4_mtd_unmount_littlefs_mount_block_device(void)
-{
-	if ((param_instance == -1) || (param_part == -1) || (param_block == -1)) {
-		PX4_ERR("MTD_PARAMETERS never initialized");
-		return -1;
-	}
-
-	char blockname[32];
-	snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", param_block);
-
-	// in case LittleFS is mounted, unmount it
-	nx_umount2(instances[param_instance]->partition_names[param_part], 0);
-	unregister_mtddriver(blockname);
-
-	int ret = ftl_initialize(0, instances[param_instance]->part_dev[param_part]);
-
-	if (ret < 0) {
-		PX4_ERR("ftl_initialize failed with error %d, param_block %d, param_instance %d, param_part %d, block_counts %d",
-			ret, param_block, param_instance, param_part, *instances[param_instance]->partition_block_counts);
-
-	} else {
-		ret = bchdev_register(blockname, instances[param_instance]->partition_names[param_part], false);
-
-		if (ret < 0) {
-			PX4_ERR("bchdev_register failed: %d", ret);
-
-		}
-	}
-
-	return ret;
-}
-
-int px4_mtd_unmount_block_device_mount_littlefs(void)
-{
-	if ((param_instance == -1) || (param_part == -1) || (param_block == -1)) {
-		PX4_ERR("MTD_PARAMETERS never initialized");
-		return -1;
-	}
-
-	char blockname[32];
-	snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", param_block);
-
-	int ret =  bchdev_unregister(instances[param_instance]->partition_names[param_part]);
-
-	if (ret < 0) {
-		PX4_ERR("bchdev_unregister %s failed: %d", instances[param_instance]->partition_names[param_part], ret);
-
-	} else {
-		ret = unregister_blockdriver(blockname);
-
-		if (ret < 0) {
-			PX4_ERR("unregister_blockdriver %s failed: %d", blockname, ret);
-
-		} else {
-			ret = px4_mtd_forceformat_littlefs();
-		}
-	}
-
-	return ret;
-}
-
-int px4_mtd_forceformat_littlefs(void)
-{
-	char blockname[32];
-	snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", param_block);
-
-	int ret = register_mtddriver(blockname, instances[param_instance]->part_dev[param_part], 0755, nullptr);
-
-	if (ret < 0) {
-		PX4_ERR("register_mtddriver %s failed: %d", blockname, ret);
-
-	} else {
-		ret = nx_mount(blockname, instances[param_instance]->partition_names[param_part], "littlefs", 0, "forceformat");
-
-		if (ret < 0) {
-			PX4_ERR("nx_mount %s failed: %d", instances[param_instance]->partition_names[param_part], ret);
-
-		}
-	}
-
-	return ret;
-}
-
-#endif // CONFIG_MTD
@@ -53,7 +53,7 @@

 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 #include <lockstep_scheduler/lockstep_scheduler.h>
-static LockstepScheduler lockstep_scheduler {};
+static LockstepScheduler lockstep_scheduler {true};
 #endif

 // Intervals in usec
@@ -46,7 +46,7 @@
 class LockstepComponents
 {
 public:
-	LockstepComponents();
+	LockstepComponents(bool no_cleanup_on_destroy = false);
 	~LockstepComponents();

 	/**
@@ -69,6 +69,7 @@ public:
 	void wait_for_components();

 private:
+	const bool _no_cleanup_on_destroy;

 	px4_sem_t _components_sem;

@@ -46,6 +46,7 @@
 class LockstepScheduler
 {
 public:
+	LockstepScheduler(bool no_cleanup_on_destroy = false) : _components(no_cleanup_on_destroy) {}
 	~LockstepScheduler();

 	void set_absolute_time(uint64_t time_us);
@@ -42,14 +42,19 @@
 #include <px4_platform_common/tasks.h>
 #include <limits.h>

-LockstepComponents::LockstepComponents()
+LockstepComponents::LockstepComponents(bool no_cleanup_on_destroy)
+	: _no_cleanup_on_destroy(no_cleanup_on_destroy)
 {
 	px4_sem_init(&_components_sem, 0, 0);
 }

 LockstepComponents::~LockstepComponents()
 {
-	px4_sem_destroy(&_components_sem);
+	// Trying to destroy a condition variable with threads currently blocked on it results in undefined behavior.
+	// Therefore we allow the caller not to cleanup and let the OS take care of that.
+	if (!_no_cleanup_on_destroy) {
+		px4_sem_destroy(&_components_sem);
+	}
 }

 int LockstepComponents::register_component()
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- * Copyright (C) 2022 ModalAI, Inc. All rights reserved.
+ * Copyright (C) 2022-2023 ModalAI, Inc. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -39,7 +39,9 @@

 __BEGIN_DECLS

-//Defining hap_debug
+extern void qurt_log_to_apps(int level, const char *message);
+
+// Defining hap_debug
 void HAP_debug(const char *msg, int level, const char *filename, int line);

 static __inline void qurt_log(int level, const char *file, int line,
@@ -51,6 +53,8 @@ static __inline void qurt_log(int level, const char *file, int line,
 	vsnprintf(buf, sizeof(buf), format, args);
 	va_end(args);
 	HAP_debug(buf, level, file, line);
+
+	qurt_log_to_apps(level, buf);
 }

 __END_DECLS
@@ -37,6 +37,7 @@ set(QURT_LAYER_SRCS
 	tasks.cpp
 	px4_qurt_impl.cpp
 	main.cpp
+	qurt_log.cpp
 	)

 add_library(px4_layer
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023 ModalAI, Inc. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -30,48 +30,19 @@
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
+#include <px4_platform_common/log.h>
+#include <uORB/uORBManager.hpp>

-#include <nuttx/spi/spi.h>
-#include <px4_platform_common/px4_manifest.h>
-//                                                              KiB BS    nB
-static const px4_mft_device_t spifram = {             // FM25V05 on FMUM 32K 512 X 128
-	.bus_type = px4_mft_device_t::SPI,
-	.devid    = SPIDEV_FLASH(0)
-};
-
-static const px4_mtd_entry_t fmum_fram = {
-	.device = &spifram,
-	.npart = 1,
-	.partd = {
-		{
-			.type = MTD_PARAMETERS,
-			.path = "/fs/mtd_params",
-			.nblocks = 128
-			//.nblocks = 4096
-		},
-	},
-};
-
-static const px4_mtd_manifest_t board_mtd_config = {
-	.nconfigs = 1,
-	.entries  = {
-		&fmum_fram
-	}
-};
-
-static const px4_mft_entry_s mtd_mft = {
-	.type = MTD,
-	.pmft = (void *) &board_mtd_config,
-};
-
-static const px4_mft_s mft = {
-	.nmft = 1,
-	.mfts = {
-		&mtd_mft
-	}
-};
-
-const px4_mft_s *board_get_manifest(void)
+// This function will send a debug or error message up to the apps proc
+// so that it can be displayed and logged. Otherwise the messages are only
+// available with the mini-dm tool that requires adb (i.e. USB cable attached)
+extern "C" void qurt_log_to_apps(int level, const char *message)
 {
-	return &mft;
-}
+	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
+
+	if (ch != nullptr) {
+		if (level >= _PX4_LOG_LEVEL_ERROR) { ch->send_message("slpi_error", strlen(message) + 1, (uint8_t *) message); }
+
+		else { ch->send_message("slpi_debug", strlen(message) + 1, (uint8_t *) message); }
+	}
+}
@@ -80,7 +80,7 @@
 #define DRV_IMU_DEVTYPE_ICM42605 0x29
 #define DRV_IMU_DEVTYPE_ICM42670P 0x2A
 #define DRV_IMU_DEVTYPE_IIM42652 0x2B
-
+#define DRV_IMU_DEVTYPE_IAM20680HP 0x2C
 #define DRV_RNG_DEVTYPE_MB12XX   0x31
 #define DRV_RNG_DEVTYPE_LL40LS   0x32
 #define DRV_ACC_DEVTYPE_MPU6050  0x33
@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__imu__invensense__iam20680hp
+	MAIN iam20680hp
+	COMPILE_FLAGS
+	SRCS
+		IAM20680HP.cpp
+		IAM20680HP.hpp
+		iam20680hp_main.cpp
+		InvenSense_IAM20680HP_registers.hpp
+	DEPENDS
+		drivers_accelerometer
+		drivers_gyroscope
+		px4_work_queue
+	)
@@ -0,0 +1,641 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+#include "IAM20680HP.hpp"
+
+using namespace time_literals;
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+IAM20680HP::IAM20680HP(const I2CSPIDriverConfig &config) :
+	SPI(config),
+	I2CSPIDriver(config),
+	_drdy_gpio(config.drdy_gpio),
+	_px4_accel(get_device_id(), config.rotation),
+	_px4_gyro(get_device_id(), config.rotation)
+{
+	if (_drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
+
+	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+}
+
+IAM20680HP::~IAM20680HP()
+{
+	perf_free(_bad_register_perf);
+	perf_free(_bad_transfer_perf);
+	perf_free(_fifo_empty_perf);
+	perf_free(_fifo_overflow_perf);
+	perf_free(_fifo_reset_perf);
+	perf_free(_drdy_missed_perf);
+}
+
+int IAM20680HP::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool IAM20680HP::Reset()
+{
+	_state = STATE::RESET;
+	DataReadyInterruptDisable();
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void IAM20680HP::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void IAM20680HP::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+
+	perf_print_counter(_bad_register_perf);
+	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_drdy_missed_perf);
+}
+
+bool IAM20680HP::StoreCheckedRegisterValue(Register reg)
+{
+	// 3 retries
+	for (int i = 0; i < 3; i++) {
+		uint8_t read1 = RegisterRead(reg);
+		uint8_t read2 = RegisterRead(reg);
+
+		if (read1 == read2) {
+			for (auto &r : _register_cfg) {
+				if (r.reg == reg) {
+					r.set_bits = read1;
+					r.clear_bits = ~read1;
+					return true;
+				}
+			}
+
+		} else {
+			PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast<uint8_t>(reg), read1, read2);
+		}
+	}
+
+	return false;
+}
+
+int IAM20680HP::probe()
+{
+	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+
+	if (whoami != WHOAMI) {
+		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+void IAM20680HP::RunImpl()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::RESET:
+		// PWR_MGMT_1: Device Reset
+		RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
+		_reset_timestamp = now;
+		_failure_count = 0;
+		_state = STATE::WAIT_FOR_RESET;
+		ScheduleDelayed(100_ms);
+		break;
+
+	case STATE::WAIT_FOR_RESET:
+
+		// The reset value is 0x00 for all registers other than the registers below
+		//  Document Number: DS-000114 Page Page 35 of 53
+		if (RegisterRead(Register::WHO_AM_I) == WHOAMI) {
+
+			// offset registers (factory calibration) should not change during normal operation
+			StoreCheckedRegisterValue(Register::XA_OFFSET_H);
+			StoreCheckedRegisterValue(Register::XA_OFFSET_L);
+			StoreCheckedRegisterValue(Register::YA_OFFSET_H);
+			StoreCheckedRegisterValue(Register::YA_OFFSET_L);
+			StoreCheckedRegisterValue(Register::ZA_OFFSET_H);
+			StoreCheckedRegisterValue(Register::ZA_OFFSET_L);
+
+			// Wakeup and reset digital signal path
+			RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
+			RegisterWrite(Register::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST);
+			RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST | USER_CTRL_BIT::I2C_IF_DIS);
+
+			// if reset succeeded then configure
+			_state = STATE::CONFIGURE;
+			ScheduleDelayed(35_ms); // max 35 ms start-up time from sleep
+
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Reset failed, retrying");
+				_state = STATE::RESET;
+				ScheduleDelayed(100_ms);
+
+			} else {
+				PX4_DEBUG("Reset not complete, check again in 10 ms");
+				ScheduleDelayed(10_ms);
+			}
+		}
+
+		break;
+
+	case STATE::CONFIGURE:
+		if (Configure()) {
+			// if configure succeeded then start reading from FIFO
+			_state = STATE::FIFO_READ;
+
+			if (DataReadyInterruptConfigure()) {
+				_data_ready_interrupt_enabled = true;
+
+				// backup schedule as a watchdog timeout
+				ScheduleDelayed(100_ms);
+
+			} else {
+				_data_ready_interrupt_enabled = false;
+				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+			}
+
+			FIFOReset();
+
+		} else {
+			// CONFIGURE not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Configure failed, resetting");
+				_state = STATE::RESET;
+
+			} else {
+				PX4_DEBUG("Configure failed, retrying");
+			}
+
+			ScheduleDelayed(100_ms);
+		}
+
+		break;
+
+	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			// always check current FIFO count
+			bool success = false;
+			const uint16_t fifo_count = FIFOReadCount();
+
+			if (fifo_count >= FIFO::SIZE) {
+				FIFOReset();
+				perf_count(_fifo_overflow_perf);
+
+			} else if (fifo_count == 0) {
+				perf_count(_fifo_empty_perf);
+
+			} else {
+				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+					samples--;
+				}
+
+				if (samples > FIFO_MAX_SAMPLES) {
+					// not technically an overflow, but more samples than we expected or can publish
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
+						success = true;
+
+						if (_failure_count > 0) {
+							_failure_count--;
+						}
+					}
+				}
+			}
+
+			if (!success) {
+				_failure_count++;
+
+				// full reset if things are failing consistently
+				if (_failure_count > 10) {
+					Reset();
+					return;
+				}
+			}
+
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				// check configuration registers periodically or immediately following any failure
+				if (RegisterCheck(_register_cfg[_checked_register])) {
+					_last_config_check_timestamp = now;
+					_checked_register = (_checked_register + 1) % size_register_cfg;
+
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
+
+			} else {
+				// periodically update temperature (~1 Hz)
+				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
+					UpdateTemperature();
+					_temperature_update_timestamp = now;
+				}
+			}
+		}
+
+		break;
+	}
+}
+
+void IAM20680HP::ConfigureAccel()
+{
+	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
+
+	switch (ACCEL_FS_SEL) {
+	case ACCEL_FS_SEL_2G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
+		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
+		break;
+
+	case ACCEL_FS_SEL_4G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
+		break;
+
+	case ACCEL_FS_SEL_8G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
+		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
+		break;
+
+	case ACCEL_FS_SEL_16G:
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		break;
+	}
+}
+
+void IAM20680HP::ConfigureGyro()
+{
+	const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0]
+
+	float range_dps = 0.f;
+
+	switch (FS_SEL) {
+	case FS_SEL_250_DPS:
+		range_dps = 250.f;
+		break;
+
+	case FS_SEL_500_DPS:
+		range_dps = 500.f;
+		break;
+
+	case FS_SEL_1000_DPS:
+		range_dps = 1000.f;
+		break;
+
+	case FS_SEL_2000_DPS:
+		range_dps = 2000.f;
+		break;
+	}
+
+	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));	//灵敏度为16.4 LSB/dps
+	_px4_gyro.set_range(math::radians(range_dps));	//陀螺仪量程
+}
+
+void IAM20680HP::ConfigureSampleRate(int sample_rate)
+{
+	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
+	const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
+
+	// recompute FIFO empty interval (us) with actual gyro sample limit
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
+}
+
+bool IAM20680HP::Configure()
+{
+	// first set and clear all configured register bits
+	for (const auto &reg_cfg : _register_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	// now check that all are configured
+	bool success = true;
+
+	for (const auto &reg_cfg : _register_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	ConfigureAccel();
+	ConfigureGyro();
+
+	return success;
+}
+
+int IAM20680HP::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<IAM20680HP *>(arg)->DataReady();
+	return 0;
+}
+
+void IAM20680HP::DataReady()
+{
+	// at least the required number of samples in the FIFO
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
+		ScheduleNow();
+	}
+}
+
+bool IAM20680HP::DataReadyInterruptConfigure()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	// Setup data ready on falling edge
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+}
+
+bool IAM20680HP::DataReadyInterruptDisable()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+bool IAM20680HP::RegisterCheck(const register_config_t &reg_cfg)
+{
+	bool success = true;
+
+	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+
+	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+		success = false;
+	}
+
+	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+		success = false;
+	}
+
+	return success;
+}
+
+uint8_t IAM20680HP::RegisterRead(Register reg)
+{
+	uint8_t cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+void IAM20680HP::RegisterWrite(Register reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+void IAM20680HP::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
+{
+	const uint8_t orig_val = RegisterRead(reg);
+
+	uint8_t val = (orig_val & ~clearbits) | setbits;
+
+	if (orig_val != val) {
+		RegisterWrite(reg, val);
+	}
+}
+
+uint16_t IAM20680HP::FIFOReadCount()
+{
+	// read FIFO count
+	uint8_t fifo_count_buf[3] {};
+	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
+
+	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return 0;
+	}
+
+	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+}
+
+bool IAM20680HP::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
+{
+	FIFOTransferBuffer buffer{};
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+
+	ProcessGyro(timestamp_sample, buffer.f, samples);
+	return ProcessAccel(timestamp_sample, buffer.f, samples);
+}
+
+void IAM20680HP::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// FIFO_EN: disable FIFO
+	RegisterWrite(Register::FIFO_EN, 0);
+
+	// USER_CTRL: reset FIFO
+	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
+
+	// reset while FIFO is disabled
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
+
+	// FIFO_EN: enable both gyro and accel
+	// USER_CTRL: re-enable FIFO
+	for (const auto &r : _register_cfg) {
+		if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) {
+			RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
+		}
+	}
+}
+
+static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
+{
+	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
+}
+
+bool IAM20680HP::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_accel_fifo_s accel{};
+	accel.timestamp_sample = timestamp_sample;
+	accel.samples = 0;
+	accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
+
+	bool bad_data = false;
+
+	// accel data is doubled in FIFO, but might be shifted
+	int accel_first_sample = 1;
+
+	if (samples >= 4) {
+		if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
+			// [A0, A1, A2, A3]
+			//  A0==A1, A2==A3
+			accel_first_sample = 1;
+
+		} else if (fifo_accel_equal(fifo[1], fifo[2])) {
+			// [A0, A1, A2, A3]
+			//  A0, A1==A2, A3
+			accel_first_sample = 0;
+
+		} else {
+			// no matching accel samples is an error
+			bad_data = true;
+			perf_count(_bad_transfer_perf);
+		}
+	}
+
+	for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
+		int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
+		int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
+		int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
+
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		accel.x[accel.samples] = accel_x;
+		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
+		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+		accel.samples++;
+	}
+
+	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+
+	if (accel.samples > 0) {
+		_px4_accel.updateFIFO(accel);
+	}
+
+	return !bad_data;
+}
+
+void IAM20680HP::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_gyro_fifo_s gyro{};
+	gyro.timestamp_sample = timestamp_sample;
+	gyro.samples = samples;
+	gyro.dt = FIFO_SAMPLE_DT;
+
+	for (int i = 0; i < samples; i++) {
+		const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
+		const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
+		const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
+
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		gyro.x[i] = gyro_x;
+		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+	}
+
+	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+
+	_px4_gyro.updateFIFO(gyro);
+}
+
+void IAM20680HP::UpdateTemperature()
+{
+	// read current temperature
+	uint8_t temperature_buf[3] {};
+	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
+
+	if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return;
+	}
+
+	const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
+	const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+	if (PX4_ISFINITE(TEMP_degC)) {
+		_px4_accel.set_temperature(TEMP_degC);
+		_px4_gyro.set_temperature(TEMP_degC);
+	}
+}
@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+
+#pragma once
+
+#include "InvenSense_IAM20680HP_registers.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/device/spi.h>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <lib/drivers/device/Device.hpp>
+
+using namespace InvenSense_IAM20680HP;
+
+class IAM20680HP : public device::SPI, public I2CSPIDriver<IAM20680HP>
+{
+public:
+	IAM20680HP(const I2CSPIDriverConfig &config);
+	~IAM20680HP() override;
+
+	static void print_usage();
+
+	void RunImpl();
+
+	int init() override;
+	void print_status() override;
+
+private:
+	void exit_and_cleanup() override;
+
+	// Sensor Configuration
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                     // ensure at least 1 new accel sample per transfer
+	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};              // 8000 Hz gyro
+	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER};  // 4000 Hz accel
+
+	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+
+	// Transfer data
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	};
+	// ensure no struct padding
+	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
+	struct register_config_t {
+		Register reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	int probe() override;
+
+	bool Reset();
+
+	bool Configure();
+	void ConfigureAccel();
+	void ConfigureGyro();
+	void ConfigureSampleRate(int sample_rate);
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	bool RegisterCheck(const register_config_t &reg_cfg);
+	bool StoreCheckedRegisterValue(Register reg);
+
+	uint8_t RegisterRead(Register reg);
+	void RegisterWrite(Register reg, uint8_t value);
+	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+	void FIFOReset();
+
+	bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	void UpdateTemperature();
+
+	const spi_drdy_gpio_t _drdy_gpio;
+
+	PX4Accelerometer _px4_accel;
+	PX4Gyroscope _px4_gyro;
+
+	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
+	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
+	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
+	perf_counter_t _drdy_missed_perf{nullptr};
+
+	hrt_abstime _reset_timestamp{0};
+	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+	int _failure_count{0};
+
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
+	bool _data_ready_interrupt_enabled{false};
+
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_READ,
+	} _state{STATE::RESET};
+
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+
+	uint8_t _checked_register{0};
+	static constexpr uint8_t size_register_cfg{16};
+	register_config_t _register_cfg[size_register_cfg] {
+		// Register                | Set bits, Clear bits
+		{ Register::CONFIG,         CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 },
+		{ Register::SMPLRT_DIV,     SMPLRT_DIV_BIT::SPEED, 0 },//输出速率100hz
+		{ Register::GYRO_CONFIG,    GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },//陀螺仪±2000dps量程
+		{ Register::ACCEL_CONFIG,   ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 },//加速度计±16G量程
+		{ Register::ACCEL_CONFIG2,  ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B, ACCEL_CONFIG2_BIT::FIFO_SIZE },//加速度计低通滤波BW=21.2Hz
+		{ Register::FIFO_EN,        FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN },
+		{ Register::INT_PIN_CFG,    INT_PIN_CFG_BIT::INT_LEVEL, 0 },
+		{ Register::INT_ENABLE,     INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 },
+		{ Register::USER_CTRL,      USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
+		{ Register::PWR_MGMT_1,     PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP },
+		{ Register::XA_OFFSET_H,    0, 0 },
+		{ Register::XA_OFFSET_L,    0, 0 },
+		{ Register::YA_OFFSET_H,    0, 0 },
+		{ Register::YA_OFFSET_L,    0, 0 },
+		{ Register::ZA_OFFSET_H,    0, 0 },
+		{ Register::ZA_OFFSET_L,    0, 0 },
+	};
+};
@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+#pragma once
+
+#include <cstdint>
+
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+namespace InvenSense_IAM20680HP
+{
+static constexpr uint32_t SPI_SPEED = 8 * 1000 * 1000; // 8MHz SPI serial interface
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t WHOAMI = 0xF8; //whoami默认值
+
+static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C 灵敏度
+static constexpr float TEMPERATURE_OFFSET = 25.f; // C 温度偏移
+//寄存器地址
+enum class Register : uint8_t {
+	SMPLRT_DIV        = 0X19,
+	CONFIG            = 0x1A,
+	GYRO_CONFIG       = 0x1B,
+	ACCEL_CONFIG      = 0x1C,
+	ACCEL_CONFIG2     = 0x1D,
+
+	FIFO_EN           = 0x23,
+
+	INT_PIN_CFG       = 0x37,
+	INT_ENABLE        = 0x38,
+
+	TEMP_OUT_H        = 0x41,
+	TEMP_OUT_L        = 0x42,
+
+	SIGNAL_PATH_RESET = 0x68,
+
+	USER_CTRL         = 0x6A,
+	PWR_MGMT_1        = 0x6B,
+
+	FIFO_COUNTH       = 0x72,
+	FIFO_COUNTL       = 0x73,
+	FIFO_R_W          = 0x74,
+	WHO_AM_I          = 0x75,
+
+	XA_OFFSET_H       = 0x77,
+	XA_OFFSET_L       = 0x78,
+
+	YA_OFFSET_H       = 0x7A,
+	YA_OFFSET_L       = 0x7B,
+
+	ZA_OFFSET_H       = 0x7D,
+	ZA_OFFSET_L       = 0x7E,
+};
+
+// CONFIG
+enum CONFIG_BIT : uint8_t {
+	FIFO_MODE = Bit6, // when the FIFO is full, additional writes will not be written to FIFO
+
+	DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz [2:0]
+};
+
+// GYRO_CONFIG
+enum GYRO_CONFIG_BIT : uint8_t {
+	// FS_SEL [4:3]
+	FS_SEL_250_DPS	= 0,           // 0b00000
+	FS_SEL_500_DPS	= Bit3,        // 0b01000
+	FS_SEL_1000_DPS	= Bit4,        // 0b10000
+	FS_SEL_2000_DPS	= Bit4 | Bit3, // 0b11000
+
+	// FCHOICE_B [1:0]
+	FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0   8 kHz
+};
+
+// ACCEL_CONFIG
+enum ACCEL_CONFIG_BIT : uint8_t {
+	// ACCEL_FS_SEL [4:3]
+	ACCEL_FS_SEL_2G  = 0,           // 0b00000
+	ACCEL_FS_SEL_4G  = Bit3,        // 0b01000
+	ACCEL_FS_SEL_8G  = Bit4,        // 0b10000
+	ACCEL_FS_SEL_16G = Bit4 | Bit3, // 0b11000
+};
+
+// ACCEL_CONFIG2
+enum ACCEL_CONFIG2_BIT : uint8_t {
+	FIFO_SIZE       = Bit7 | Bit6, // 0=512bytes,
+	ACCEL_FCHOICE_B = Bit3,        // Used to bypass DLPF (DS-000114 Page 40 of 53)
+};
+
+// FIFO_EN
+enum FIFO_EN_BIT : uint8_t {
+	TEMP_FIFO_EN  = Bit7,
+	XG_FIFO_EN    = Bit6,
+	YG_FIFO_EN    = Bit5,
+	ZG_FIFO_EN    = Bit4,
+	ACCEL_FIFO_EN = Bit3,
+};
+
+// INT_PIN_CFG
+enum INT_PIN_CFG_BIT : uint8_t {
+	INT_LEVEL = Bit7,
+};
+
+// INT_ENABLE
+enum INT_ENABLE_BIT : uint8_t {
+	DATA_RDY_INT_EN = Bit0,
+};
+
+// SIGNAL_PATH_RESET
+enum SIGNAL_PATH_RESET_BIT : uint8_t {
+	ACCEL_RST = Bit1,
+	TEMP_RST  = Bit0,
+};
+
+// USER_CTRL
+enum USER_CTRL_BIT : uint8_t {
+	FIFO_EN      = Bit6,
+	I2C_IF_DIS   = Bit4,
+	FIFO_RST     = Bit2,
+	SIG_COND_RST = Bit0,
+};
+
+// PWR_MGMT_1
+enum PWR_MGMT_1_BIT : uint8_t {
+	DEVICE_RESET = Bit7,
+	SLEEP        = Bit6,
+
+	// CLKSEL[2:0]
+	CLKSEL_0     = Bit0, // It is required that CLKSEL[2:0] be set to 001 to achieve full gyroscope performance.
+};
+enum SMPLRT_DIV_BIT : uint8_t {
+	SPEED     = Bit3 | Bit0, // It is required that CLKSEL[2:0] be set to 001 to achieve full gyroscope performance.
+};
+namespace FIFO
+{
+static constexpr size_t SIZE = 512; // max is 4 KB, but limited in software to 512 bytes via ACCEL_CONFIG2
+
+// FIFO_DATA layout when FIFO_EN has both {X, Y, Z}G_FIFO_EN and ACCEL_FIFO_EN set
+struct DATA {
+	uint8_t ACCEL_XOUT_H;
+	uint8_t ACCEL_XOUT_L;
+	uint8_t ACCEL_YOUT_H;
+	uint8_t ACCEL_YOUT_L;
+	uint8_t ACCEL_ZOUT_H;
+	uint8_t ACCEL_ZOUT_L;
+	uint8_t GYRO_XOUT_H;
+	uint8_t GYRO_XOUT_L;
+	uint8_t GYRO_YOUT_H;
+	uint8_t GYRO_YOUT_L;
+	uint8_t GYRO_ZOUT_H;
+	uint8_t GYRO_ZOUT_L;
+};
+}
+
+} // namespace InvenSense_IAM20680HP
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_IMU_INVENSENSE_IAM20680HP
+	bool "iam20680hp"
+	default n
+	---help---
+		Enable support for iam20680hp
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Konrad	ebe6d57fb8	[Mission]: Update the mission mode to use the planned mission interface library	2023-02-01 09:03:33 +01:00
Konrad	f8bcd673ab	[Mission] Add planned mission interface to easily extract data from dataman.	2023-02-01 09:03:33 +01:00
Silvan Fuhrer	cbc4c35bcf	Commander params: improve meta data for max flight time and wind Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 08:48:09 +01:00
Silvan Fuhrer	6d84da5cf1	Commander: add max flight time warning (starting at 90%) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-02-01 08:48:09 +01:00
Beat Küng	b1709743f7	commander: add wind or flight time limit exceeded mode requirement This prevents switching into any of these modes once the condition is set.	2023-02-01 08:48:09 +01:00
Beat Küng	a8628c9d9c	fix commander: clear takeoff_time only on disarm Otherwise the flight time restriction flag gets cleared too early, before disarming (which puts the vehicle into the previous mode and it might take off again).	2023-02-01 08:48:09 +01:00
Beat Küng	8e4c5884ec	fix commander: need to check for valid mode change even if already the same Fixes the following case: - user intention set to X - failsafe triggers, mode = Y - can_run for X becomes false - user tries to switch to X -> need to re-evaluate can_run	2023-02-01 08:48:09 +01:00
Beat Küng	7988491e37	failsafe simulator: improve spacing for multi-line checkboxes Reduces line-height to 100% in that case.	2023-02-01 08:48:09 +01:00
ShiauweiZhao	931f602995	drivers/imu/invensense: new IAM-20680HP IMU driver (#21025 )	2023-01-31 09:33:26 -05:00
Daniel Agar	2be701f902	platforms/nuttx: cmake debug pass GDB path and set RTOS plugin file extension	2023-01-30 20:19:38 -05:00
Eric Katzfey	a4aa76f0ac	VOXL2 board updates and new Kconfig option for ROOTFSDIR - also includes a couple of miscellaneous changes to VOXL2 support to show Qurt messages on px4 console and put logs in the proper spot	2023-01-30 12:03:40 -05:00
Julian Oes	b1fc0ca0d0	mavlink: always forward messages to/from USB Previously, it was not possible to enable forwarding of messages to/from teh USB instance because it does not have a param for it, like the serial instances have. With this commit, we change the default to always set forwarding on for the USB instance as that is likely desired. Signed-off-by: Julian Oes <julian@oes.ch>	2023-01-30 10:46:14 -05:00
Julian Oes	0446292c75	mavlink: always use public method Signed-off-by: Julian Oes <julian@oes.ch>	2023-01-30 10:46:14 -05:00
Julian Oes	7b8cf4913e	mavlink: show forwarding status Signed-off-by: Julian Oes <julian@oes.ch>	2023-01-30 10:46:14 -05:00
Daniel Agar	47215bb625	lib/wind_estimator: symforce codegen remove reserved identifier naming - from the C standard, "All identifiers that begin with an underscore and either an uppercase letter or another underscore are always reserved for any use"	2023-01-30 09:24:32 -05:00
Daniel Agar	efe1d43550	ekf2: symforce codegen remove reserved identifier naming - from the C standard, "All identifiers that begin with an underscore and either an uppercase letter or another underscore are always reserved for any use"	2023-01-30 09:24:32 -05:00
Beat Küng	0687fd2689	lockstep_scheduler: avoid pthread_cond_destroy on at_exit The static object is destroyed on at_exit while threads might still be inside a CS. This can lead to a hanging process. Cleaner would be to gracefully stop the threads. According to https://linux.die.net/man/3/pthread_cond_destroy: Attempting to destroy a condition variable upon which other threads are currently blocked results in undefined behavior.	2023-01-30 11:45:02 +01:00
Hamish Willee	f25abbc80a	Fix magnetometer typo	2023-01-30 10:24:16 +01:00
Daniel Agar	8da993c30e	Update world_magnetic_model to latest Sat Jan 28 11:14:05 UTC 2023 Co-authored-by: PX4 BuildBot <bot@px4.io>	2023-01-28 11:41:03 -05:00
PX4 BuildBot	88038717dc	update all px4board kconfig	2023-01-28 11:39:51 -05:00
Roman Bapst	4646762f9d	Use multiplication instead of division	2023-01-28 10:31:17 -05:00
Silvan Fuhrer	0c735dea2e	AirspeedSelector: use scientific notation for small param Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-01-28 10:31:17 -05:00
Silvan Fuhrer	d32f400851	WindEstimator: use isAllFinite() Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-01-28 10:31:17 -05:00
Silvan Fuhrer	a6d14796e4	WindEstimator: add consts, fix float comparison to 0 and use consistently floats in division Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-01-28 10:31:17 -05:00
Silvan Fuhrer	15335b194a	WindEstimator: use state indexing enum consistently Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-01-28 10:31:17 -05:00
PX4 BuildBot	21ddb04856	Update submodule mavlink to latest Sat Jan 28 00:39:00 UTC 2023 - mavlink in PX4/Firmware (fa65292bb7542cb1f2a4cdaac10738fa17777a6d): https://github.com/mavlink/mavlink/commit/74dee05f0cd121ae27e021d011a04b161c9d0440 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/e3b8756e37cd2cd01ba658461bb4dbffb2fdf914 - Changes: https://github.com/mavlink/mavlink/compare/74dee05f0cd121ae27e021d011a04b161c9d0440...e3b8756e37cd2cd01ba658461bb4dbffb2fdf914 e3b8756e 2023-01-25 olliw42 - update storm32.xml (#1941)	2023-01-27 21:14:07 -05:00