ekf2: optical flow control always use provided flow gyro (with bias applied)

ekf2: log optical flow gyro bias
ekf2: access state covariance using helper functions
2026-07-02 20:10:34 +08:00 · 2023-09-26 10:57:57 -04:00 · 2023-09-26 10:57:19 -04:00 · 2023-09-26 06:41:04 +02:00 · 2023-09-25 12:26:29 -04:00 · 2023-09-25 12:26:12 -04:00
138 changed files with 1730 additions and 1209 deletions
@@ -29,7 +29,8 @@
 		"twxs.cmake",
 		"uavcan.dsdl",
 		"wholroyd.jinja",
-		"zixuanwang.linkerscript"
+		"zixuanwang.linkerscript",
+		"ms-vscode.makefile-tools"
 	],

 	"containerUser": "user",
@@ -18,6 +18,7 @@
        "twxs.cmake",
        "uavcan.dsdl",
        "wholroyd.jinja",
-        "zixuanwang.linkerscript"
+        "zixuanwang.linkerscript",
+        "ms-vscode.makefile-tools"
    ]
 }
@@ -414,6 +414,8 @@ endif()
 #
 add_library(parameters_interface INTERFACE)
 add_library(kernel_parameters_interface INTERFACE)
+add_library(events_interface INTERFACE)
+add_library(kernel_events_interface INTERFACE)

 include(px4_add_library)
 add_subdirectory(src/lib EXCLUDE_FROM_ALL)
@@ -440,8 +442,11 @@ add_subdirectory(src/lib/parameters EXCLUDE_FROM_ALL)
 if(${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT)
 	target_link_libraries(parameters_interface INTERFACE usr_parameters)
 	target_link_libraries(kernel_parameters_interface INTERFACE parameters)
+	target_link_libraries(events_interface INTERFACE usr_events)
+	target_link_libraries(kernel_events_interface INTERFACE events)
 else()
 	target_link_libraries(parameters_interface INTERFACE parameters)
+	target_link_libraries(events_interface INTERFACE events)
 endif()

 # firmware added last to generate the builtin for included modules
@@ -38,5 +38,3 @@ param set-default PWM_MAIN_FUNC1 101
 param set-default PWM_MAIN_FUNC2 102
 param set-default PWM_MAIN_FUNC3 103
 param set-default PWM_MAIN_FUNC4 104
-param set-default PWM_MAIN_TIM0 -4
-
@@ -140,6 +140,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -115,6 +115,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -117,6 +117,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -117,6 +117,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -117,6 +117,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -189,6 +189,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 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 512 X 64
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -45,18 +45,12 @@ 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 = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT))
 		}
 	},
 };
@@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -42,18 +42,12 @@ static const px4_mft_device_t i2c1 = {             // 24AA64FT on Base  8K 32 X

 static const px4_mtd_entry_t fmu_eeprom = {
 	.device = &i2c1,
-	.npart = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
 			.nblocks = 128
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 128
-
 		}
 	},
 };
@@ -119,6 +119,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -132,6 +132,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -120,6 +120,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -149,6 +149,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -150,6 +150,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -149,6 +149,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -138,6 +138,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=131072
 CONFIG_RAM_START=0x20000000
@@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -152,6 +152,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -34,25 +34,19 @@
 #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 512 X 64
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi5,
-	.npart = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT))
 		}
 	},
 };
@@ -149,6 +149,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -168,6 +168,8 @@ CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x1fff0000
@@ -171,6 +171,8 @@ CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x1fff0000
@@ -171,6 +171,8 @@ CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x1fff0000
@@ -170,6 +170,8 @@ CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x1fff0000
@@ -171,6 +171,8 @@ CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x1fff0000
@@ -184,6 +184,8 @@ CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=1048576
 CONFIG_RAM_START=0x20200000
@@ -136,6 +136,8 @@ CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_DEFAULT_PRIO_INHERIT=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -140,6 +140,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -140,6 +140,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=393216
 CONFIG_RAM_START=0x20000000
@@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=393216
 CONFIG_RAM_START=0x20000000
@@ -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 = {             // FM25V01A on FMUM 16K
+static const px4_mft_device_t spi2 = {             // FM25V01A on FMUM native: 16K X 8, emulated as (512 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -46,7 +46,7 @@ static const px4_mtd_entry_t fmum_fram = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
+			.nblocks = (16384 / (1 << CONFIG_RAMTRON_EMULATE_PAGE_SHIFT))
 		},
 	},
 };
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -192,6 +192,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -152,6 +152,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -147,6 +147,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -146,6 +146,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -3,6 +3,7 @@ CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_DISTANCE_SENSOR=n
 CONFIG_COMMON_HYGROMETERS=n
+CONFIG_COMMON_RC=n
 CONFIG_COMMON_TELEMETRY=n
 CONFIG_DRIVERS_ADC_ADS1115=n
 CONFIG_DRIVERS_BATT_SMBUS=n
@@ -185,6 +185,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -183,6 +183,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 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 512 X 64
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -50,18 +50,12 @@ 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 = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT))
 		}
 	},
 };
@@ -151,6 +151,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 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 512 X 64
+static const px4_mft_device_t spi2 = {             // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -47,18 +47,12 @@ 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 = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT))
 		}
 	},
 };
@@ -151,6 +151,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -34,25 +34,19 @@
 #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 512 X 64
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &spi5,
-	.npart = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT))
 		}
 	},
 };
@@ -19,16 +19,18 @@ CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
 CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
-CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_COMMON_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
@@ -48,24 +48,32 @@ then
 fi


-if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004
+if ver hwtypecmp V6X000006 V6X004006 V6X005006
 then
-	# Internal SPI bus ICM20649
-	icm20649 -s -R 6 start
+	# Internal SPI bus ICM45686
+	icm45686 -s -R 10 start
+	iim42652 -s -R 6 start
+	adis16470 -s -R 0 start
 else
-	# Internal SPI BMI088
-	if ver hwtypecmp V6X009010 V6X010010
+	if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004
 	then
-		bmi088 -A -R 6 -s start
-		bmi088 -G -R 6 -s start
+		# Internal SPI bus ICM20649
+		icm20649 -s -R 6 start
 	else
-		if ver hwtypecmp V6X000010
+		# Internal SPI BMI088
+		if ver hwtypecmp V6X009010 V6X010010
 		then
-			bmi088 -A -R 0 -s start
-			bmi088 -G -R 0 -s start
+			bmi088 -A -R 6 -s start
+			bmi088 -G -R 6 -s start
 		else
-			bmi088 -A -R 4 -s start
-			bmi088 -G -R 4 -s start
+			if ver hwtypecmp V6X000010
+			then
+				bmi088 -A -R 0 -s start
+				bmi088 -G -R 0 -s start
+			else
+				bmi088 -A -R 4 -s start
+				bmi088 -G -R 4 -s start
+			fi
 		fi
 	fi
 fi
@@ -112,7 +120,7 @@ ist8310 -X -b 1 -R 10 start
 # Possible internal Baro
 if param compare SENS_INT_BARO_EN 1
 then
-	if ver hwtypecmp V6X002001
+	if ver hwtypecmp V6X002001 V6X000006 V6X004006 V6X005006
 	then
 		icp201xx -I -a 0x64 start
 	else
@@ -189,6 +189,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -215,24 +215,28 @@
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
 #define HW_INFO_INIT_PREFIX    "V6X"

-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 11 // Rev 0 and Rev 3,4 Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 13 // Rev 0 and Rev 3,4,6 Sensor sets
 //                 Base/FMUM
 #define V6X00   HW_VER_REV(0x0,0x0) // FMUV6X,                 Rev 0
 #define V6X01   HW_VER_REV(0x0,0x1) // FMUV6X,     BMI388 I2C2 Rev 1
 #define V6X03   HW_VER_REV(0x0,0x3) // FMUV6X,     Sensor Set  Rev 3
 #define V6X04   HW_VER_REV(0x0,0x4) // FMUV6X,     Sensor Set  Rev 4
+#define V6X06   HW_VER_REV(0x0,0x6) // FMUV6X,     Sensor Set  Rev 6
 #define V6X10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
 #define V6X13   HW_VER_REV(0x1,0x3) // NO PX4IO,   Sensor Set  Rev 3
 #define V6X14   HW_VER_REV(0x1,0x4) // NO PX4IO,   Sensor Set  Rev 4
+#define V6X16   HW_VER_REV(0x1,0x6) // NO PX4IO,   Sensor Set  Rev 6
 #define V6X21   HW_VER_REV(0x2,0x1) // FMUV6X,     CUAV Sensor Set
 #define V6X40   HW_VER_REV(0x4,0x0) // FMUV6X,                    HB CM4 base Rev 0
 #define V6X41   HW_VER_REV(0x4,0x1) // FMUV6X,     BMI388 I2C2    HB CM4 base Rev 1
 #define V6X43   HW_VER_REV(0x4,0x3) // FMUV6X,     Sensor Set     HB CM4 base Rev 3
 #define V6X44   HW_VER_REV(0x4,0x4) // FMUV6X,     Sensor Set     HB CM4 base Rev 4
+#define V6X46   HW_VER_REV(0x4,0x6) // FMUV6X,     Sensor Set     HB CM4 base Rev 6
 #define V6X50   HW_VER_REV(0x5,0x0) // FMUV6X,                    HB Mini Rev 0
 #define V6X51   HW_VER_REV(0x5,0x1) // FMUV6X,     BMI388 I2C2    HB Mini Rev 1
 #define V6X53   HW_VER_REV(0x5,0x3) // FMUV6X,     Sensor Set     HB Mini Rev 3
 #define V6X54   HW_VER_REV(0x5,0x4) // FMUV6X,     Sensor Set     HB Mini Rev 4
+#define V6X56   HW_VER_REV(0x5,0x6) // FMUV6X,     Sensor Set     HB Mini Rev 6
 #define V6X90   HW_VER_REV(0x9,0x0) //                         Rev 0
 #define V6X0910   HW_VER_REV(0x9,0x10)  // FMUV6X,     rev from EEPROM     Auterion Skynode ver9
 #define V6X1010   HW_VER_REV(0x10,0x10) // FMUV6X,     rev from EEPROM     Auterion Skynode ver10
@@ -142,17 +142,20 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
 	{V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2
 	{V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3
+	{V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4
+	{V6X06, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6
 	{V6X40, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, //                       HB CM4 base
 	{V6X41, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2  HB CM4 base
 	{V6X43, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3
 	{V6X44, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4
+	{V6X46, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6
 	{V6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, //                       HB Mini
 	{V6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2  HB Mini
 	{V6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3
 	{V6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4
+	{V6X56, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 6
 	{V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO
 	{V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3
-	{V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4
 	{V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4
 	{V6X21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
 	{V6X0910, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X,     rev from EEPROM     Auterion Skynode ver9
@@ -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 512 X 64
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -50,18 +50,12 @@ 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 = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT))
 		}
 	},
 };
@@ -84,6 +84,29 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),

+	initSPIHWVersion(V6X06, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
 	initSPIHWVersion(V6X21, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
@@ -132,6 +155,52 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),

+	initSPIHWVersion(V6X44, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
+	initSPIHWVersion(V6X46, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
 	initSPIHWVersion(V6X50, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
@@ -156,75 +225,6 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),

-	initSPIHWVersion(V6X44, {
-		initSPIBus(SPI::Bus::SPI1, {
-			initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
-		}, {GPIO::PortI, GPIO::Pin11}),
-		initSPIBus(SPI::Bus::SPI2, {
-			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
-		}, {GPIO::PortF, GPIO::Pin4}),
-		initSPIBus(SPI::Bus::SPI3, {
-			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
-		}, {GPIO::PortE, GPIO::Pin7}),
-		//  initSPIBus(SPI::Bus::SPI4, {
-		//    // no devices
-		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
-		//  }, {GPIO::PortG, GPIO::Pin8}),
-		initSPIBus(SPI::Bus::SPI5, {
-			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
-		}),
-		initSPIBusExternal(SPI::Bus::SPI6, {
-			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
-			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
-		}),
-	}),
-
-	// never shipped
-	//initSPIHWVersion(V6X50, {
-	//	initSPIBus(SPI::Bus::SPI1, {
-	//		initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
-	//	}, {GPIO::PortI, GPIO::Pin11}),
-	//	initSPIBus(SPI::Bus::SPI2, {
-	//		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
-	//	}, {GPIO::PortF, GPIO::Pin4}),
-	//	initSPIBus(SPI::Bus::SPI3, {
-	//		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
-	//		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
-	//	}, {GPIO::PortE, GPIO::Pin7}),
-	//	//  initSPIBus(SPI::Bus::SPI4, {
-	//	//    // no devices
-	//	// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
-	//	//  }, {GPIO::PortG, GPIO::Pin8}),
-	//	initSPIBus(SPI::Bus::SPI5, {
-	//		initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
-	//	}),
-	//	initSPIBusExternal(SPI::Bus::SPI6, {
-	//		initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
-	//		initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
-	//	}),
-	//}),
-	initSPIHWVersion(V6X04, {
-		initSPIBus(SPI::Bus::SPI1, {
-			initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
-		}, {GPIO::PortI, GPIO::Pin11}),
-		initSPIBus(SPI::Bus::SPI2, {
-			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
-		}, {GPIO::PortF, GPIO::Pin4}),
-		initSPIBus(SPI::Bus::SPI3, {
-			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
-		}, {GPIO::PortE, GPIO::Pin7}),
-		//  initSPIBus(SPI::Bus::SPI4, {
-		//    // no devices
-		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
-		//  }, {GPIO::PortG, GPIO::Pin8}),
-		initSPIBus(SPI::Bus::SPI5, {
-			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
-		}),
-		initSPIBusExternal(SPI::Bus::SPI6, {
-			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
-			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
-		}),
-	}),
 	initSPIHWVersion(V6X53, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
@@ -248,6 +248,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
 		}),
 	}),
+
 	initSPIHWVersion(V6X54, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
@@ -270,6 +271,30 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
 		}),
 	}),
+
+	initSPIHWVersion(V6X56, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
 	initSPIHWVersion(V6X0910, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
@@ -293,6 +318,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
 		}),
 	}),
+
 	initSPIHWVersion(V6X1010, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
@@ -148,6 +148,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=245760
 CONFIG_RAM_START=0x20010000
@@ -181,6 +181,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 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 512 X 64
+static const px4_mft_device_t spi5 = {             // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };
@@ -45,18 +45,12 @@ 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 = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT))
 		}
 	},
 };
@@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -141,6 +141,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -140,6 +140,8 @@ CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
 CONFIG_PTHREAD_MUTEX_ROBUST=y
 CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
 CONFIG_RAMTRON_SETSPEED=y
 CONFIG_RAM_SIZE=262144
 CONFIG_RAM_START=0x20000000
@@ -107,6 +107,7 @@ function(px4_add_functional_gtest)
 		                                              px4_daemon
 		                                              work_queue
 		                                              parameters
+		                                              events
 		                                              perf
 		                                              tinybson
 		                                              uorb_msgs
@@ -168,10 +168,11 @@ function(px4_add_module)
 	if(NOT DYNAMIC)
 		target_link_libraries(${MODULE} PRIVATE prebuild_targets px4_platform systemlib perf)
 		if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT AND KERNEL)
-			target_link_libraries(${MODULE} PRIVATE kernel_parameters_interface px4_kernel_layer uORB_kernel)
+			target_link_libraries(${MODULE} PRIVATE
+				kernel_events_interface kernel_parameters_interface px4_kernel_layer uORB_kernel)
 			set_property(GLOBAL APPEND PROPERTY PX4_KERNEL_MODULE_LIBRARIES ${MODULE})
 		else()
-			target_link_libraries(${MODULE} PRIVATE parameters_interface px4_layer uORB)
+			target_link_libraries(${MODULE} PRIVATE events_interface parameters_interface px4_layer uORB)
 			set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE})
 		endif()
 		set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR})
@@ -10,4 +10,6 @@ float32[2] flow_compensated_integral   # integrated optical flow measurement com
 float32[3] gyro_rate                   # gyro measurement synchronized with flow measurements (rad/s)
 float32[3] gyro_rate_integral          # gyro measurement integrated to flow rate and synchronized with flow measurements (rad)

+float32[3] gyro_bias
+
 # TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel
@@ -43,7 +43,6 @@ endif()
 add_library(px4_platform STATIC
 	board_common.c
 	board_identity.c
-	events.cpp
 	external_reset_lockout.cpp
 	i2c.cpp
 	i2c_spi_buses.cpp
@@ -53,7 +53,8 @@
 #define _CRYPTOIOCBASE			IOCTL_IDX_TO_BASE(2)
 #define _PARAMIOCBASE			IOCTL_IDX_TO_BASE(3)
 #define _PLATFORMIOCBASE			IOCTL_IDX_TO_BASE(4)
-#define MAX_IOCTL_PTRS 5
+#define _EVENTSIOCBASE			IOCTL_IDX_TO_BASE(5)
+#define MAX_IOCTL_PTRS 6

 /* The PLATFORMIOCLAUNCH IOCTL is used to launch kernel side modules
 * from the user side code
@@ -38,6 +38,7 @@
 #include <px4_platform_common/console_buffer.h>
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
+#include <lib/events/events.h>
 #include <lib/parameters/param.h>
 #include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
 #include <px4_platform/cpuload.h>
@@ -125,6 +126,7 @@ int px4_platform_init()

 #if !defined(CONFIG_BUILD_FLAT)
 	hrt_ioctl_init();
+	events_ioctl_init();
 #endif

 	/* configure CPU load estimation */
@@ -249,25 +249,19 @@ static const px4_mtd_manifest_t default_mtd_config = {

 #else

-const px4_mft_device_t spifram  = {             // FM25V02A on FMUM 32K 512 X 64
+const px4_mft_device_t spifram  = {             // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
 	.bus_type = px4_mft_device_t::SPI,
 	.devid    = SPIDEV_FLASH(0)
 };

 const px4_mtd_entry_t fram = {
 	.device = &spifram,
-	.npart = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT))
 		}
 	},
 };
@@ -52,6 +52,8 @@ target_link_libraries(px4_kernel_layer
 		nuttx_kc
 		nuttx_karch
 		nuttx_kmm
+	PRIVATE
+		kernel_events_interface # events_ioctl_init
 )

 target_link_libraries(px4_kernel_layer
@@ -110,3 +110,13 @@ add_custom_command(OUTPUT ${generated_events_header}
 )
 add_custom_target(events_header DEPENDS ${generated_events_header})
 add_dependencies(prebuild_targets events_header)
+
+# Build the interface(s)
+if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT)
+	list(APPEND EXTRA_SRCS events_ioctl.cpp)
+	add_library(usr_events usr_events.cpp)
+	add_dependencies(usr_events prebuild_targets)
+endif()
+
+add_library(events events.cpp ${EXTRA_SRCS})
+add_dependencies(events prebuild_targets)
@@ -0,0 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 Technology Innovation Institute. 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
+
+void events_ioctl_init(void);
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 Technology Innovation Institute. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file events_ioctl.cpp
+ *
+ * Interface to send events from user space.
+ */
+
+#include <px4_platform/board_ctrl.h>
+
+#include <errno.h>
+
+#include "events_ioctl.h"
+
+static int events_ioctl(unsigned int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	switch (cmd) {
+	case EVENTSIOCSEND: {
+			eventiocsend_t *data = (eventiocsend_t *)arg;
+			events::send(data->event);
+		}
+		break;
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	return ret;
+}
+
+void events_ioctl_init(void)
+{
+	px4_register_boardct_ioctl(_EVENTSIOCBASE, events_ioctl);
+}
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 Technology Innovation Institute. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file events_ioctl.h
+ *
+ * User space - kernel space interface for dispatching events.
+ */
+
+#pragma once
+
+#include <px4_platform/board_ctrl.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/events.h>
+
+#define _EVENTSIOC(_n) (_PX4_IOC(_EVENTSIOCBASE, _n))
+
+#define EVENTSIOCSEND _EVENTSIOC(1)
+typedef struct eventiocsend {
+	events::EventType &event;
+} eventiocsend_t;
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 Technology Innovation Institute. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file usr_events.cpp
+ *
+ * User space interface for dispatching events.
+ */
+
+#include <sys/boardctl.h>
+
+#include "events_ioctl.h"
+
+namespace events
+{
+
+void send(EventType &event)
+{
+	eventiocsend_t data = {event};
+	boardctl(EVENTSIOCSEND, reinterpret_cast<unsigned long>(&data));
+}
+
+} /* namespace events */
@@ -49,6 +49,7 @@ class SlewRate
 {
 public:
 	SlewRate() = default;
+	SlewRate(Type initial_value) { setForcedValue(initial_value); }
 	~SlewRate() = default;

 	/**
@@ -194,15 +194,15 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour

 	_fault_status.flags.bad_airspeed = false;

-	Vector24f H; // Observation jacobian
-	Vector24f K; // Kalman gain vector
+	VectorState H; // Observation jacobian
+	VectorState K; // Kalman gain vector

 	sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K);

 	if (update_wind_only) {
-		for (unsigned row = 0; row <= 21; row++) {
-			K(row) = 0.f;
-		}
+		const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);
+		K.setZero();
+		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}

 	const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation);
@@ -258,7 +258,7 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample
 	const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw;

 	// it is safer to remove all existing correlations to other states at this time
-	P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
+	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.0f);

 	P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw);
 	P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) -
@@ -227,7 +227,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}

 	// predict the covariance
-	SquareMatrix24f nextP;
+	SquareMatrixState nextP;

 	// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
 	sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P,
@@ -298,7 +298,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)

 	if (_control_status.flags.wind) {
 		const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
-;
+
 		for (unsigned index = 0; index < State::wind_vel.dof; index++) {
 			unsigned i = State::wind_vel.idx + index;
 			nextP(i, i) += wind_vel_process_noise;
@@ -511,7 +511,7 @@ void Ekf::constrainStateVar(const IdxDof &state, float min, float max)

 // if the covariance correction will result in a negative variance, then
 // the covariance matrix is unhealthy and must be corrected
-bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
+bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
 {
 	bool healthy = true;

@@ -527,24 +527,24 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)

 void Ekf::resetQuatCov(const float yaw_noise)
 {
-	const float tilt_var = sq(fmaxf(_params.initial_tilt_err, 1.0e-2f));
-	Vector3f rot_var_ned(tilt_var, tilt_var, 0.f);
+	const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));
+	float yaw_var = sq(0.01f);

 	// update the yaw angle variance using the variance of the measurement
 	if (PX4_ISFINITE(yaw_noise)) {
 		// using magnetic heading tuning parameter
-		rot_var_ned(2) = (sq(fmaxf(yaw_noise, 1.0e-2f)));
+		yaw_var = math::max(sq(yaw_noise), yaw_var);
 	}

-	// clear existing quaternion covariance
-	// Optimization: avoid the creation of a <4> function
-	P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx, 0.0f);
-	P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx + 2, 0.0f);
+	resetQuatCov(Vector3f(tilt_var, tilt_var, yaw_var));
+}

+void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
+{
 	matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov;
 	sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov);
 	q_cov.copyLowerToUpperTriangle();
-	P.slice<State::quat_nominal.dof, State::quat_nominal.dof>(State::quat_nominal.idx, State::quat_nominal.idx) = q_cov;
+	resetStateCovariance<State::quat_nominal>(q_cov);
 }

 void Ekf::resetMagCov()
@@ -560,8 +560,8 @@ void Ekf::resetMagCov()

 	saveMagCovData();
 #else
-	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
-	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
+	P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.f);
+	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.f);

 #endif
 }
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	064ea0d465	ekf2: optical flow control always use provided flow gyro (with bias applied)	2023-09-26 10:57:57 -04:00
Daniel Agar	291af17b64	ekf2: log optical flow gyro bias	2023-09-26 10:57:19 -04:00
bresch	99197919d7	ekf2: access state covariance using helper functions	2023-09-26 06:41:04 +02:00
Daniel Agar	619616b9f0	vscode add Makefile extension to recommended and devcontainer.json	2023-09-25 12:26:29 -04:00
Daniel Agar	aa97ef9d50	EKF: covariance remove extra semicolon	2023-09-25 12:26:12 -04:00
Mathieu Bresciani	b3f460f30c	ekf2: update quaternion covariance at yaw reset(#22123 ) - Preserve tilt variance while doing a yaw reset - Yaw variance is now correctly set instead of increased --------- Co-authored-by: Dominique <dominique.leblanc@donecle.com> Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>	2023-09-25 09:36:42 -04:00
bresch	514e0330e5	ekf2_terrain: handle height reset	2023-09-25 09:34:14 -04:00
Ville Juven	6cb2c176d5	events: Move implementation of events::send() to lib/events Events have a global, system-wide sequence number, which must be handled atomically, (fetching and incrementing the sequence AND sending the event to uORB must be atomic). Currently in FLAT mode, only one instance of this sequence number exists, so it is OK to have it in px4_platform. However, in PROTECTED mode px4_platform is instantiated both in kernel- and user spaces, which makes two instances of this sequence number, which causes problems in the mavlink event handling logic. When mavlink receives and handles events, it expects that: - The sequence numbers arrive in order (seq n is followed by n+1 etc) - It increments by 1 - There is only one instance of the sequence number In PROTECTED mode this is violated, as the kernel and user sequence numbers run freely on their own. This patch fixes the issue by moving the event backend to the kernel and by providing user access to it via ioctl.	2023-09-25 09:54:11 +02:00
Vincent Poon	fafec397e8	Fix Default Output Protocol - Airframe 4019_x500_v2 Remove "param set-default PWM_MAIN_TIM0 -4"	2023-09-25 09:28:47 +02:00
Daniel Agar	61aee73b91	boards: px4_fmu-v5_stackcheck disable common RC to save flash	2023-09-22 11:19:29 -04:00
bresch	db97a38a9d	ekf2 rng kin: allow check to become true during horizontal motion Even if there is some horizontal motion, a passing check should be accepted as the terrain can be flat. However, the vehicle must not be moving horizontally to invalidate the consistency as a change in terrain can make the kinematic check temporarily fail.	2023-09-22 09:02:35 -04:00
David Sidrane	e920bfb188	px4_fmuv6x:Fit Rev6 Sensors	2023-09-21 16:22:44 -04:00
David Sidrane	9151d582ed	px4_fmu-v6:Add Sensor Set Rev 6	2023-09-21 16:22:44 -04:00
Silvan Fuhrer	5590ab3caa	FW Position Controller: handle IDLE waypoints in FW_POSCTRL_MODE_AUTO, also with NAN setpoints (#22114 ) A setpoint of type IDLE can be published by Navigator without a valid position, and should be handled in the auto function in FW_POSCTRL_MODE_AUTO. If it wouldn't be handled there the controller would switch to mode FW_POSCTRL_MODE_OTHER, in which case no attitude setpoint is published and the lower controllers would be stuck with the last published value (incl. thrust). Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-09-21 17:27:54 +02:00
Silvan Fuhrer	acec07fb25	Update src/modules/mc_att_control/mc_att_control_main.cpp Co-authored-by: Matthias Grob <maetugr@gmail.com>	2023-09-21 17:25:53 +02:00
Matthias Grob	710f977349	mc_att_control: ramp thrust in after spoolup time	2023-09-21 17:25:53 +02:00
Matthias Grob	4ca366c04f	mc_att_control: respect spoolup time in Stabilized mode	2023-09-21 17:25:53 +02:00
Matthias Grob	24111df176	mc_att_control_main: zero minimal thrust when landed but this time compared to `2fbb70d9ca` the minimum thrust is ramping to not produce a thrust jump. The better long term solution will be to always have airmode but with the effect limited to a magnitude of MPC_MANTHR_MIN.	2023-09-21 17:25:53 +02:00
Jari Nippula	7239e8473e	microxrce_client to use px4_poll for uorb subs Subscribe to poll uorbs instead of sleeping px4_usleep to allow more accurate timing.	2023-09-21 08:30:28 -04:00
bresch	9e962f3efa	ekf2: update more hardcoded indexes	2023-09-21 08:27:52 -04:00
bresch	ebf962bf68	ekf2: remove size in name of state vector and matrix types Then the state vector size can be changes without having to update the name	2023-09-21 08:27:52 -04:00
RomanBapst	4f1682c3c8	UTM_GLOBAL_POSITION: prevent uint16 overflow Signed-off-by: RomanBapst <bapstroman@gmail.com>	2023-09-21 08:21:56 -04:00
Matthias Grob	be56f74c1d	mc_acro_params: beginner/tuning friendly defaults high rates and (super) expo are necessary for - acrobatic flying - fpv - racing but they are not useful for: - a typical vehicle's rate control tuning - beginners The defaults that I set in #8036 were put with the assumption Acro mode is mainly used on racing drones for acrobatic manouvers but instead users including me use it most of the time to tune any drone. When tuning the rates are lowered and expo disabled. My suggested strategy is to make the beginner friendly rates without expo which are good for tuning the default and document more clearly how this can be raised for acrobatic flying in documentation and later on hopefully also directly in the ground station UI on an Acro mode specific page.	2023-09-20 17:45:07 -04:00
Matthias Grob	77c2df2e04	mc_acro_params: separate parameter group	2023-09-20 17:45:07 -04:00
Matthias Grob	189fa6831a	mc_acro_params: more readable descriptions	2023-09-20 17:45:07 -04:00
Matthias Grob	df3a0de734	mc_rate_control: move acro parameter to separate file	2023-09-20 17:45:07 -04:00
Igor Mišić	c1dbe177b8	boards: move FRAM emulated sector size to the 32-byte granularity (#21204 )	2023-09-20 16:50:36 -04:00
Silvan Fuhrer	ec7db4b30d	FW Positon Controller: set references to 0 if not provided by local_position (#22101 ) * FW Positon Controller: set altitude_ref to 0 if not provided by GPS Signed-off-by: Silvan Fuhrer <silvan@auterion.com> * FW Positon Controller: set lat/lon reference to 0 if not provided in local_position Signed-off-by: Silvan Fuhrer <silvan@auterion.com> --------- Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-09-20 15:45:32 +02:00