.github/workflows/compile_nuttx.yml

@@ -45,7 +45,6 @@ jobs:
           matek_h743-slim,
           modalai_fc-v1,
           modalai_fc-v2,
-          mro_ctrl-zero-classic,
           mro_ctrl-zero-f7,
           mro_ctrl-zero-f7-oem,
           mro_ctrl-zero-h7,

ROMFS/px4fmu_common/init.d/rcS

@@ -162,7 +162,7 @@ else
 		param select-backup $PARAM_BACKUP_FILE
 	fi
 
-	if mft query -q -k MFT -s MFT_ETHERNET -v 1
+	if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X ARK_FMU_V6X
 	then
 		netman update -i eth0
 	fi
@@ -266,9 +266,12 @@ else
 		. $FCONFIG
 	fi
 
-	if px4io supported
+	#
+	# Start IO for PWM output or RC input if enabled
+	#
+	if param compare -s SYS_USE_IO 1
 	then
-	# Check if PX4IO present and update firmware if needed.
+		# Check if PX4IO present and update firmware if needed.
 		if [ -f $IOFW ]
 		then
 			if ! px4io checkcrc ${IOFW}
@@ -290,12 +293,12 @@ else
 					tune_control stop
 				fi
 			fi
+		fi
 
-			if ! px4io start
-			then
-				echo "PX4IO start failed"
-				set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
-			fi
+		if ! px4io start
+		then
+			echo "PX4IO start failed"
+			set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
 		fi
 	fi
 

boards/ark/fmu-v6x/init/rc.board_defaults

@@ -22,14 +22,21 @@ param set-default SENS_IMU_TEMP 10.0
 #param set-default SENS_IMU_TEMP_I 0.025
 #param set-default SENS_IMU_TEMP_P 1.0
 
-if ver hwtypecmp ARKV6X000
+if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
 then
 	param set-default SENS_TEMP_ID 2818058
 fi
 
-if ver hwtypecmp ARKV6X001
+if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001
 then
 	param set-default SENS_TEMP_ID 3014666
 fi
 
+if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007
+then
+	param set-default SYS_USE_IO 0
+else
+	param set-default SYS_USE_IO 1
+fi
+
 safety_button start

boards/ark/fmu-v6x/init/rc.board_sensors

@@ -4,7 +4,7 @@
 #------------------------------------------------------------------------------
 set HAVE_PM2 yes
 
-if mft query -q -k MFT -s MFT_PM2 -v 0
+if ver hwtypecmp ARKV6X005000 ARKV6X005001 ARKV6X005002 ARKV6X005003 ARKV6X005004
 then
 	set HAVE_PM2 no
 fi
@@ -47,7 +47,7 @@ then
 	fi
 fi
 
-if ver hwtypecmp ARKV6X000
+if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
 then
 	# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz
 	iim42652 -R 3 -s -b 1 -C 32051 start
@@ -59,7 +59,7 @@ then
 	icm42688p -R 6 -s -b 3 -C 32051 start
 fi
 
-if ver hwtypecmp ARKV6X001
+if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001
 then
 	# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
 	iim42653 -R 3 -s -b 1 -C 32051 start

boards/ark/fmu-v6x/src/CMakeLists.txt

@@ -55,6 +55,7 @@ else()
 		init.c
 		led.c
 		mtd.cpp
+		manifest.c
 		sdio.c
 		spi.cpp
 		spix_sync.c

boards/ark/fmu-v6x/src/board_config.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016-2024 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2022 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
@@ -204,17 +204,25 @@
 #define BOARD_ADC_OPEN_CIRCUIT_V     (5.6f)
 
 /* HW Version and Revision drive signals Default to 1 to detect */
-#define BOARD_HAS_HW_SPLIT_VERSIONING
+#define BOARD_HAS_HW_VERSIONING
 
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PH4 */  GPIO_ADC3_INP15
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
 #define HW_INFO_INIT_PREFIX    "ARKV6X"
 
-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 1
 //                 Base/FMUM
-#define ARKV6X_0   HW_FMUM_ID(0x0) // ARKV6X,     Sensor Set  Rev 0
-#define ARKV6X_1   HW_FMUM_ID(0x1) // ARKV6X,     Sensor Set  Rev 1
+#define ARKV6X00   HW_VER_REV(0x0,0x0) // ARKV6X,     Sensor Set  Rev 0
+#define ARKV6X01   HW_VER_REV(0x0,0x1) // ARKV6X,     Sensor Set  Rev 1
+//#define ARKV6X03   HW_VER_REV(0x0,0x3) // ARKV6X,     Sensor Set  Rev 3
+//#define ARKV6X04   HW_VER_REV(0x0,0x4) // ARKV6X,     Sensor Set  Rev 4
+#define ARKV6X10   HW_VER_REV(0x1,0x0) // NO PX4IO,   Sensor Set  Rev 0
+#define ARKV6X11   HW_VER_REV(0x1,0x1) // NO PX4IO,   Sensor Set  Rev 1
+#define ARKV6X40   HW_VER_REV(0x4,0x0) // ARKV6X,     Sensor Set  Rev 0   HB CM4 base Rev 3
+#define ARKV6X41   HW_VER_REV(0x4,0x1) // ARKV6X,     Sensor Set  Rev 1   HB CM4 base Rev 4
+#define ARKV6X50   HW_VER_REV(0x5,0x0) // ARKV6X,     Sensor Set  Rev 0   HB Mini Rev 5
+#define ARKV6X51   HW_VER_REV(0x5,0x1) // ARKV6X,     Sensor Set  Rev 1   HB Mini Rev 1 // never shipped
 
 #define UAVCAN_NUM_IFACES_RUNTIME  1
 
@@ -239,6 +247,7 @@
 /* PWM
  */
 #define DIRECT_PWM_OUTPUT_CHANNELS   8
+#define BOARD_PWM_FREQ	             1024000
 
 #define GPIO_FMU_CH1                    /* PI0  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0)
 #define GPIO_FMU_CH2                    /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12)

boards/ark/fmu-v6x/src/manifest.c

@@ -0,0 +1,216 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manifest.c
+ *
+ * This module supplies the interface to the manifest of hardware that is
+ * optional and dependent on the HW REV and HW VER IDs
+ *
+ * The manifest allows the system to know whether a hardware option
+ * say for example the PX4IO is an no-pop option vs it is broken.
+ *
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <board_config.h>
+
+#include <inttypes.h>
+#include <stdbool.h>
+#include <syslog.h>
+
+#include "systemlib/px4_macros.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+typedef struct {
+	uint32_t                hw_ver_rev; /* the version and revision */
+	const px4_hw_mft_item_t *mft;       /* The first entry */
+	uint32_t                entries;    /* the lenght of the list */
+} px4_hw_mft_list_entry_t;
+
+typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
+#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1
+
+static const px4_hw_mft_item_t device_unsupported = {0, 0, 0};
+
+// List of components on a specific board configuration
+// The index of those components is given by the enum (px4_hw_mft_item_id_t)
+// declared in board_common.h
+static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0640[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0650[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+};
+
+
+static px4_hw_mft_list_entry_t mft_lists[] = {
+//  ver_rev
+	{ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X,     Sensor Set  Rev 0
+	{ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X,     Sensor Set  Rev 1
+	{ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO,   Sensor Set  Rev 0
+	{ARKV6X11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO,   Sensor Set  Rev 1
+	{ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X,     Sensor Set  Rev 0   HB CM4 base Rev 3
+	{ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X,     Sensor Set  Rev 1   HB CM4 base Rev 4
+	{ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X,     Sensor Set  Rev 0   HB Mini Rev 5
+	{ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X,     Sensor Set  Rev 1   HB Mini Rev 1 // never shipped
+};
+
+/************************************************************************************
+ * Name: board_query_manifest
+ *
+ * Description:
+ *   Optional returns manifest item.
+ *
+ * Input Parameters:
+ *   manifest_id - the ID for the manifest item to retrieve
+ *
+ * Returned Value:
+ *   0 - item is not in manifest => assume legacy operations
+ *   pointer to a manifest item
+ *
+ ************************************************************************************/
+
+__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
+{
+	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
+
+	if (boards_manifest == px4_hw_mft_list_uninitialized) {
+		uint32_t ver_rev = board_get_hw_version() << 16;
+		ver_rev |= board_get_hw_revision();
+
+		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
+			if (mft_lists[i].hw_ver_rev == ver_rev) {
+				boards_manifest = &mft_lists[i];
+				break;
+			}
+		}
+
+		if (boards_manifest == px4_hw_mft_list_uninitialized) {
+			syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev);
+		}
+	}
+
+	px4_hw_mft_item rv = &device_unsupported;
+
+	if (boards_manifest != px4_hw_mft_list_uninitialized &&
+	    id < boards_manifest->entries) {
+		rv = &boards_manifest->mft[id];
+	}
+
+	return rv;
+}

boards/ark/fmu-v6x/src/mtd.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2024 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2022 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
@@ -31,9 +31,6 @@
  *
  ****************************************************************************/
 
-#include <nuttx/config.h>
-#include <board_config.h>
-
 #include <nuttx/spi/spi.h>
 #include <px4_platform_common/px4_manifest.h>
 //                                                              KiB BS    nB
@@ -95,16 +92,10 @@ static const px4_mft_entry_s mtd_mft = {
 	.pmft = (void *) &board_mtd_config,
 };
 
-static const px4_mft_entry_s mft_mft = {
-	.type = MFT,
-	.pmft = (void *) system_query_manifest,
-};
-
 static const px4_mft_s mft = {
-	.nmft = 2,
+	.nmft = 1,
 	.mfts = {
-		&mtd_mft,
-		&mft_mft,
+		&mtd_mft
 	}
 };
 

boards/ark/fmu-v6x/src/spi.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2024 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2022 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
@@ -36,7 +36,7 @@
 #include <nuttx/spi/spi.h>
 
 constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
-	initSPIFmumID(ARKV6X_0, {
+	initSPIHWVersion(ARKV6X00, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
 		}, {GPIO::PortI, GPIO::Pin11}),
@@ -59,7 +59,145 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),
 
-	initSPIFmumID(ARKV6X_1, {
+	initSPIHWVersion(ARKV6X01, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, 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(ARKV6X10, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, 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_ICM42688P, 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(ARKV6X11, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, 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(ARKV6X40, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, 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_ICM42688P, 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(ARKV6X41, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, 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(ARKV6X50, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, 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_ICM42688P, 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(ARKV6X51, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
 		}, {GPIO::PortI, GPIO::Pin11}),

boards/ark/fmu-v6x/src/spix_sync.c

@@ -82,7 +82,9 @@
 #define rDMAR(_tmr)     REG(_tmr, STM32_GTIM_DMAR_OFFSET)
 #define rBDTR(_tmr)     REG(_tmr, STM32_ATIM_BDTR_OFFSET)
 
-#define BOARD_SPIX_SYNC_PWM_FREQ 1024000
+#if !defined(BOARD_PWM_FREQ)
+#define BOARD_PWM_FREQ 1000000
+#endif
 
 unsigned
 spix_sync_timer_get_period(unsigned timer)
@@ -127,11 +129,11 @@ static void spix_sync_timer_init_timer(unsigned timer, unsigned rate)
 		 * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly.
 		 */
 
-		rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_SPIX_SYNC_PWM_FREQ) - 1;
+		rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1;
 
 		/* configure the timer to update at the desired rate */
 
-		rARR(timer) = (BOARD_SPIX_SYNC_PWM_FREQ / rate) - 1;
+		rARR(timer) = (BOARD_PWM_FREQ / rate) - 1;
 
 		/* generate an update event; reloads the counter and all registers */
 		rEGR(timer) = GTIM_EGR_UG;

boards/cubepilot/cubeorange/init/rc.board_defaults

@@ -14,4 +14,6 @@ param set-default SENS_EN_THERMAL 0
 
 param set-default -s SENS_TEMP_ID 2621474
 
+param set-default SYS_USE_IO 1
+
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"

boards/cubepilot/cubeorangeplus/init/rc.board_defaults

@@ -14,4 +14,6 @@ param set-default SENS_EN_THERMAL 0
 
 param set-default -s SENS_TEMP_ID 2621474
 
+param set-default SYS_USE_IO 1
+
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"

boards/cubepilot/cubeyellow/init/rc.board_defaults

@@ -13,4 +13,6 @@ param set-default BAT2_A_PER_V 17
 # Disable IMU thermal control
 param set-default SENS_EN_THERMAL 0
 
+param set-default SYS_USE_IO 1
+
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"

boards/holybro/durandal-v1/init/rc.board_defaults

@@ -11,3 +11,5 @@ param set-default BAT2_A_PER_V 36.367515152
 
 # Enable IMU thermal control
 param set-default SENS_EN_THERMAL 1
+
+param set-default SYS_USE_IO 1

boards/holybro/kakuteh7/default.px4board

@@ -33,12 +33,6 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_AUXVEL is not set
-# CONFIG_EKF2_BARO_COMPENSATION is not set
-# CONFIG_EKF2_DRAG_FUSION is not set
-# CONFIG_EKF2_EXTERNAL_VISION is not set
-# CONFIG_EKF2_GNSS_YAW is not set
-# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y

boards/holybro/kakuteh7/init/rc.board_defaults

@@ -22,10 +22,13 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050
 
-# use EKF2 without mag
+# use the Q attitude estimator, it works w/o mag or GPS.
+param set-default SYS_MC_EST_GROUP 3
+param set-default ATT_ACC_COMP 0
+param set-default ATT_W_ACC 0.4000
+param set-default ATT_W_GYRO_BIAS 0.0000
+
 param set-default SYS_HAS_MAG 0
-# and enable gravity fusion
-param set-default EKF2_IMU_CONTROL 7
 
 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090

boards/holybro/kakuteh7/src/spi.cpp

@@ -43,7 +43,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 		initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}),
 	}),
 	initSPIBus(SPI::Bus::SPI4, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}),
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}),
 		initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}),
 		initSPIDevice(DRV_IMU_DEVTYPE_BMI270, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}),
 	}),

boards/holybro/kakuteh7mini/default.px4board

@@ -32,12 +32,6 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_AUXVEL is not set
-# CONFIG_EKF2_BARO_COMPENSATION is not set
-# CONFIG_EKF2_DRAG_FUSION is not set
-# CONFIG_EKF2_EXTERNAL_VISION is not set
-# CONFIG_EKF2_GNSS_YAW is not set
-# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y

boards/holybro/kakuteh7v2/default.px4board

@@ -33,12 +33,6 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
-# CONFIG_EKF2_AUXVEL is not set
-# CONFIG_EKF2_BARO_COMPENSATION is not set
-# CONFIG_EKF2_DRAG_FUSION is not set
-# CONFIG_EKF2_EXTERNAL_VISION is not set
-# CONFIG_EKF2_GNSS_YAW is not set
-# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y

boards/holybro/pix32v5/init/rc.board_defaults

@@ -9,5 +9,7 @@ param set-default BAT2_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152
 
+param set-default SYS_USE_IO 1
+
 rgbled_pwm start
 safety_button start

boards/mro/ctrl-zero-classic/default.px4board

@@ -1,11 +1,9 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m7"
-CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
-CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
-CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
-CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
-CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0"
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
+CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
@@ -27,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -99,3 +96,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_FAKE_GPS=y

boards/mro/ctrl-zero-classic/init/rc.board_defaults

@@ -6,5 +6,4 @@
 param set-default BAT1_V_DIV 10.1
 param set-default BAT1_A_PER_V 17
 
-param set-default GPS_2_CONFIG 202
-param set-default TEL_FRSKY_CONFIG 103
+safety_button start

boards/mro/ctrl-zero-classic/init/rc.board_sensors

@@ -16,5 +16,3 @@ icm20948 -s -b 1 -R 8 -M start
 
 # Interal DPS310 (barometer)
 dps310 -s -b 2 start
-
-safety_button start

boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig

@@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
-CONFIG_ARCH_CHIP_STM32H743ZI=y
+CONFIG_ARCH_CHIP_STM32H743II=y
 CONFIG_ARCH_CHIP_STM32H7=y
 CONFIG_ARCH_INTERRUPTSTACK=768
 CONFIG_ARMV7M_BASEPRI_WAR=y

boards/mro/ctrl-zero-classic/nuttx-config/include/board.h

@@ -222,9 +222,6 @@
 
 
 /* UART/USART */
-#define GPIO_USART1_TX	 GPIO_USART1_TX_3      /* PB6  */
-#define GPIO_USART1_RX	 GPIO_USART1_RX_3      /* PB7  */
-
 #define GPIO_USART2_TX   GPIO_USART2_TX_2      /* PD5  */
 #define GPIO_USART2_RX   GPIO_USART2_RX_2      /* PD6  */
 #define GPIO_USART2_CTS  GPIO_USART2_CTS_NSS_2 /* PD3  */
@@ -238,6 +235,9 @@
 #define GPIO_UART4_TX    GPIO_UART4_TX_2       /* PA0  */
 #define GPIO_UART4_RX    GPIO_UART4_RX_2       /* PA1  */
 
+#define GPIO_USART6_TX   0                     /* USART6 is RX-only */
+#define GPIO_USART6_RX   GPIO_USART6_RX_1      /* PC7  */
+
 #define GPIO_UART7_TX    GPIO_UART7_TX_3       /* PE8  */
 #define GPIO_UART7_RX    GPIO_UART7_RX_3       /* PE7  */
 
@@ -268,14 +268,13 @@
 #define GPIO_SPI5_MISO   GPIO_SPI5_MISO_1               /* PF8  */
 #define GPIO_SPI5_MOSI   GPIO_SPI5_MOSI_2               /* PF9  */
 
-#define GPIO_SPI6_SCK    ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13  */
-#define GPIO_SPI6_MISO   GPIO_SPI6_MISO_1               /* PG12  */
-#define GPIO_SPI6_MOSI   GPIO_SPI6_MOSI_1               /* PG14  */
-
 
 /* I2C */
 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8  */
 #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2       /* PB9  */
 
-#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2       /* PF14 */
-#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2       /* PF15 */
+#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2       /* PH7 */
+#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2       /* PH8 */
+
+#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4       /* PB6 */
+#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4       /* PB7 */

boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig

@@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
 CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
 CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
 CONFIG_ARCH_CHIP="stm32h7"
-CONFIG_ARCH_CHIP_STM32H743ZI=y
+CONFIG_ARCH_CHIP_STM32H743II=y
 CONFIG_ARCH_CHIP_STM32H7=y
 CONFIG_ARCH_INTERRUPTSTACK=768
 CONFIG_ARCH_STACKDUMP=y
@@ -190,6 +190,7 @@ CONFIG_STM32H7_DMA2=y
 CONFIG_STM32H7_DMACAPABLE=y
 CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
 CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C3=y
 CONFIG_STM32H7_I2C4=y
 CONFIG_STM32H7_I2C_DYNTIMEO=y
 CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
@@ -209,20 +210,17 @@ CONFIG_STM32H7_SPI2=y
 CONFIG_STM32H7_SPI5=y
 CONFIG_STM32H7_SPI5_DMA=y
 CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
-CONFIG_STM32H7_SPI6=y
-CONFIG_STM32H7_TIM15=y
-CONFIG_STM32H7_TIM16=y
 CONFIG_STM32H7_TIM1=y
 CONFIG_STM32H7_TIM2=y
 CONFIG_STM32H7_TIM3=y
 CONFIG_STM32H7_TIM4=y
 CONFIG_STM32H7_TIM8=y
-CONFIG_STM32H7_USART1=y
-CONFIG_STM32H7_USART2=y
-CONFIG_STM32H7_USART3=y
 CONFIG_STM32H7_UART4=y
 CONFIG_STM32H7_UART7=y
 CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
 CONFIG_STM32H7_USART_BREAKS=y
 CONFIG_STM32H7_USART_INVERT=y
 CONFIG_STM32H7_USART_SINGLEWIRE=y
@@ -252,6 +250,9 @@ CONFIG_USART3_IFLOWCONTROL=y
 CONFIG_USART3_OFLOWCONTROL=y
 CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500

boards/mro/ctrl-zero-classic/src/board_config.h

@@ -45,111 +45,95 @@
 #include <stm32_gpio.h>
 
 /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
-#define GPIO_nLED_RED   /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
-#define GPIO_nLED_GREEN /* PB5  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_nLED_RED   /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_nLED_GREEN /* PB1  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
 #define GPIO_nLED_BLUE  /* PB3  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)
 
-#define GPIO_LED_SAFETY /* PE6  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
-
 #define BOARD_HAS_CONTROL_STATUS_LEDS      1
 #define BOARD_OVERLOAD_LED     LED_RED
 #define BOARD_ARMED_STATE_LED  LED_BLUE
 
 /* ADC channels */
 #define PX4_ADC_GPIO  \
-	/* PC4 */  GPIO_ADC12_INP4,   \
+	/* PA2 */  GPIO_ADC12_INP14,  \
 	/* PA3 */  GPIO_ADC12_INP15,  \
 	/* PA4 */  GPIO_ADC12_INP18,  \
-	/* PC0 */  GPIO_ADC123_INP10, \
-	/* PC5 */  GPIO_ADC12_INP8,   \
-	/* PB0 */  GPIO_ADC12_INP9,   \
-	/* PB1 */  GPIO_ADC12_INP5
+	/* PC1 */  GPIO_ADC123_INP11
 
 /* Define Channel numbers must match above GPIO pins */
-#define ADC_BATTERY_VOLTAGE_CHANNEL       4 /* PC4 BATT_VOLT_SENS */
+#define ADC_BATTERY_VOLTAGE_CHANNEL       14 /* PA2 BATT_VOLT_SENS */
 #define ADC_BATTERY_CURRENT_CHANNEL       15 /* PA3 BATT_CURRENT_SENS */
 #define ADC_SCALED_V5_CHANNEL             18 /* PA4 VDD_5V_SENS */
-#define ADC_RC_RSSI_CHANNEL               10 /* PC0 */
-#define ADC_AIRSPEED_VOLTAGE_CHANNEL       8 /* PC5 */
-#define ADC_AUX1_VOLTAGE_CHANNEL       	   9 /* PB0 */
-#define ADC_AUX2_VOLTAGE_CHANNEL       	   5 /* PB1 */
+#define ADC_RC_RSSI_CHANNEL               11 /* PC1 */
 
 #define ADC_CHANNELS \
 	((1 << ADC_BATTERY_VOLTAGE_CHANNEL)       | \
 	 (1 << ADC_BATTERY_CURRENT_CHANNEL)       | \
 	 (1 << ADC_SCALED_V5_CHANNEL)             | \
-	 (1 << ADC_RC_RSSI_CHANNEL)		  | \
-	 (1 << ADC_AIRSPEED_VOLTAGE_CHANNEL)	  | \
-	 (1 << ADC_AUX1_VOLTAGE_CHANNEL)	  | \
-	 (1 << ADC_AUX2_VOLTAGE_CHANNEL))
+	 (1 << ADC_RC_RSSI_CHANNEL))
 
 /* HW has to large of R termination on ADC todo:change when HW value is chosen */
 #define BOARD_ADC_OPEN_CIRCUIT_V     (5.6f)
 
 /* CAN Silence: Silent mode control */
-#define GPIO_CAN1_SILENT_S0  /* PF11  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11)
-#define GPIO_CAN2_SILENT_S0  /* PF12  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12)
+#define GPIO_CAN1_SILENT_S0  /* PF5  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
+#define GPIO_CAN2_SILENT_S0  /* PF5  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
 
 /* PWM */
 #define DIRECT_PWM_OUTPUT_CHANNELS  12
 
 /* Power supply control and monitoring GPIOs */
-#define GPIO_nPOWER_IN_A                /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
+#define GPIO_nPOWER_IN_A                /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
 
 #define GPIO_VDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
 #define BOARD_NUMBER_BRICKS             1
 
 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PE4  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
 
-#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
-
 /* Define True logic Power Control in arch agnostic form */
 #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
 #define READ_VDD_3V3_SPEKTRUM_POWER_EN()   (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)
 
 /* Tone alarm output */
-#define TONE_ALARM_TIMER        16  /* timer 16 */
-#define TONE_ALARM_CHANNEL      1  /* PF6 TIM16_CH1 */
+#define TONE_ALARM_TIMER        2  /* timer 2 */
+#define TONE_ALARM_CHANNEL      1  /* PA15 TIM2_CH1 */
 
-#define GPIO_BUZZER_1           /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6)
+#define GPIO_BUZZER_1           /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
 
 #define GPIO_TONE_ALARM_IDLE    GPIO_BUZZER_1
-#define GPIO_TONE_ALARM         GPIO_TIM16_CH1OUT_2
+#define GPIO_TONE_ALARM         GPIO_TIM2_CH1OUT_2
 
 /* USB OTG FS */
 #define GPIO_OTGFS_VBUS         /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
 
 /* High-resolution timer */
 #define HRT_TIMER               3  /* use timer3 for the HRT */
-#define HRT_TIMER_CHANNEL       2 /* use capture/compare channel 1 */
+#define HRT_TIMER_CHANNEL       1 /* use capture/compare channel 1 */
 
-#define HRT_PPM_CHANNEL         /* T3C2 */  2  /* use capture/compare channel 2 */
-#define GPIO_PPM_IN             /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3
+#define HRT_PPM_CHANNEL         /* T3C3 */  3  /* use capture/compare channel 3 */
+#define GPIO_PPM_IN             /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
 
 /* RC Serial port */
-#define RC_SERIAL_PORT          "/dev/ttyS5"
+#define RC_SERIAL_PORT          "/dev/ttyS3"
 
-#define GPIO_RSSI_IN            /* PC0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0)
+#define GPIO_RSSI_IN            /* PC1  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
 
 /* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */
-#define GPIO_SAFETY_SWITCH_IN              /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
-/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */
+#define GPIO_SAFETY_SWITCH_IN              /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
+/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */
 #define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */
 
-/* No PX4IO processor present */
-#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0
-
 /* Power switch controls ******************************************************/
 #define SPEKTRUM_POWER(_on_true)           VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
 
 /*
  * Board has a separate RC_IN
  *
- * GPIO PPM_IN on PC7 T3CH2
+ * GPIO PPM_IN on PB0 T3CH3
  * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
  *   Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
  */
-#define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
+#define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
 #define SPEKTRUM_RX_AS_UART()          /* Can be left as uart */
 #define SPEKTRUM_OUT(_one_true)        px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
@@ -172,7 +156,7 @@
 #define BOARD_HAS_STATIC_MANIFEST 1
 
 
-#define BOARD_NUM_IO_TIMERS 6
+#define BOARD_NUM_IO_TIMERS 5
 
 
 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -187,12 +171,9 @@
 		GPIO_CAN2_SILENT_S0,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
-		GPIO_LEVEL_SHIFTER_OE, 		  \
 		GPIO_TONE_ALARM_IDLE,             \
 		GPIO_SAFETY_SWITCH_IN,            \
 		GPIO_OTGFS_VBUS,                  \
-		GPIO_BTN_SAFETY,                  \
-		GPIO_LED_SAFETY,                  \
 	}
 
 __BEGIN_DECLS

boards/mro/ctrl-zero-classic/src/i2c.cpp

@@ -35,5 +35,6 @@
 
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(1),
+	initI2CBusExternal(3),
 	initI2CBusExternal(4),
 };

boards/mro/ctrl-zero-classic/src/spi.cpp

@@ -37,7 +37,7 @@
 
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}),
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
 	}, {GPIO::PortE, GPIO::Pin3}),
 	initSPIBus(SPI::Bus::SPI2, {
@@ -46,10 +46,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	}),
 	initSPIBus(SPI::Bus::SPI5, {
 		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
-		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
-	}),
-	initSPIBusExternal(SPI::Bus::SPI6, {
-		initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}),
+		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
 	}),
 };
 

boards/mro/ctrl-zero-classic/src/timer_config.cpp

@@ -33,28 +33,6 @@
 
 #include <px4_arch/io_timer_hw_description.h>
 
-/* Timer allocation
- *
- * TIM1_CH4	T FMU_CH1
- * TIM1_CH3	T FMU_CH2
- * TIM1_CH2	T FMU_CH3
- * TIM1_CH1	T FMU_CH4
- *
- * TIM4_CH2	T FMU_CH5
- * TIM4_CH3	T FMU_CH6
- * TIM2_CH3	T FMU_CH7
- * TIM2_CH1	T FMU_CH8
- *
- * TIM2_CH4	T FMU_CH9
- * TIM15_CH1	T FMU_CH10
- *
- * TIM8_CH1	T FMU_CH11
- *
- * TIM4_CH4	T FMU_CH12
- *
- * TIM16_CH1	T BUZZER	- Driven by other driver
- */
-
 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 	initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
 	initIOTimer(Timer::Timer4, DMA{DMA::Index1}),

boards/mro/x21-777/init/rc.board_defaults

@@ -5,3 +5,5 @@
 
 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
+
+param set-default SYS_USE_IO 1

boards/mro/x21/init/rc.board_defaults

@@ -5,3 +5,5 @@
 
 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
+
+param set-default SYS_USE_IO 1

boards/px4/fmu-v2/init/rc.board_defaults

@@ -5,3 +5,5 @@
 
 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
+
+param set-default SYS_USE_IO 1

boards/px4/fmu-v3/init/rc.board_defaults

@@ -5,3 +5,5 @@
 
 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
+
+param set-default SYS_USE_IO 1

boards/px4/fmu-v4pro/init/rc.board_defaults

@@ -13,4 +13,6 @@ param set-default BAT2_A_PER_V 26.4
 param set-default EKF2_MULTI_IMU 2
 param set-default SENS_IMU_MODE 0
 
+param set-default SYS_USE_IO 1
+
 set LOGGER_BUF 64

boards/px4/fmu-v5/init/rc.board_defaults

@@ -9,6 +9,13 @@ param set-default BAT2_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152
 
+if ver hwtypecmp V5004000 V5006000
+then
+	param set-default SYS_USE_IO 0
+else
+	param set-default SYS_USE_IO 1
+fi
+
 if ver hwtypecmp V5005000 V5005002 V5006000 V5006002
 then
 	# CUAV V5+ (V550/V552) and V5nano (V560/V562) have 3 IMUs

boards/px4/fmu-v5x/init/rc.board_defaults

@@ -16,4 +16,6 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1
 
+param set-default SYS_USE_IO 1
+
 safety_button start

boards/px4/fmu-v5x/init/rc.board_mavlink

@@ -3,7 +3,7 @@
 # board specific MAVLink startup script.
 #------------------------------------------------------------------------------
 
-if ver hwbasecmp 008 009 00a 010
+if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001
 then
 	# Start MAVLink on the UART connected to the mission computer
 	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z

boards/px4/fmu-v5x/init/rc.board_sensors

@@ -5,7 +5,7 @@
 
 set HAVE_PM2 yes
 
-if mft query -q -k MFT -s MFT_PM2 -v 0
+if ver hwtypecmp V5X005000 V5X005001 V5X005002
 then
 	set HAVE_PM2 no
 fi
@@ -49,11 +49,33 @@ then
 	fi
 fi
 
-if ver hwbasecmp 008 009 00a 010 011
+if ver hwtypecmp V5X000000 V5X000001 V5X000002 V5X001000 V5X004000 V5X004001 V5X004002 V5X005001 V5X005002
 then
+	#FMUv5Xbase board orientation
+
+	if ver hwtypecmp V5X000000 V5X000001 V5X004000 V5X004001 V5X005001
+	then
+		# Internal SPI BMI088
+		bmi088 -A -R 4 -s start
+		bmi088 -G -R 4 -s start
+	else
+		# Internal SPI bus ICM20649
+		icm20649 -s -R 6 start
+	fi
+
+	# Internal SPI bus ICM42688p
+	icm42688p -R 6 -s start
+
+	# Internal SPI bus ICM-20602 (hard-mounted)
+	icm20602 -R 10 -s start
+
+	# Internal magnetometer on I2c
+	bmm150 -I start
+
+else
 	#SKYNODE base fmu board orientation
 
-	if ver hwtypecmp V5X000 V5X001
+	if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001
 	then
 		# Internal SPI BMI088
 		bmi088 -A -R 2 -s start
@@ -72,27 +94,6 @@ then
 	# Internal magnetometer on I2c
 	bmm150 -I -R 6 start
 
-else
-	#FMUv5Xbase board orientation
-
-	if ver hwtypecmp V5X000 V5X001
-	then
-		# Internal SPI BMI088
-		bmi088 -A -R 4 -s start
-		bmi088 -G -R 4 -s start
-	else
-		# Internal SPI bus ICM20649
-		icm20649 -s -R 6 start
-	fi
-
-	# Internal SPI bus ICM42688p
-	icm42688p -R 6 -s start
-
-	# Internal SPI bus ICM-20602 (hard-mounted)
-	icm20602 -R 10 -s start
-
-	# Internal magnetometer on I2c
-	bmm150 -I start
 fi
 
 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
@@ -104,7 +105,7 @@ ist8310 -X -b 1 -R 10 start
 if param compare SENS_INT_BARO_EN 1
 then
 	bmp388 -I -a 0x77 start
-	if ver hwtypecmp V5X000
+	if ver hwtypecmp V5X000000 V5X001000 V5X008000 V5X009000 V5X00a000
 	then
 		bmp388 -I start
 	else

boards/px4/fmu-v5x/src/CMakeLists.txt

@@ -36,6 +36,7 @@ add_library(drivers_board
 	i2c.cpp
 	init.cpp
 	led.c
+	manifest.c
 	mtd.cpp
 	sdio.c
 	spi.cpp

boards/px4/fmu-v5x/src/board_config.h

@@ -179,17 +179,31 @@
 
 /* HW Version and Revision drive signals Default to 1 to detect */
 
-#define BOARD_HAS_HW_SPLIT_VERSIONING
+#define BOARD_HAS_HW_VERSIONING
 
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PF5 */  ADC3_GPIO(15)
 #define GPIO_HW_VER_SENSE      /* PF4 */  ADC3_GPIO(14)
 #define HW_INFO_INIT_PREFIX           "V5X"
-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3
-
-#define V5X_0     HW_FMUM_ID(0x0)   // FMUV5X, Auterion     FMUv5x RC13 (baro2 BMP388 on I2C4) Sensor Set Rev 0
-#define V5X_1     HW_FMUM_ID(0x1)   // FMUV5X, Auterion, HB FMUv5x RC15 (baro2 BMP388 on I2C2) Sensor Set Rev 1
-#define V5X_2     HW_FMUM_ID(0x2)   // FMUV5X, HB           FMUv5x                             Sensor Set Rev 2
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 7
+// Base                   FMUM
+#define V5X00   HW_VER_REV(0x0,0x0) // FMUV5X,                         Rev 0
+#define V5X10   HW_VER_REV(0x1,0x0) // NO PX4IO,                       Rev 0
+#define V5X01   HW_VER_REV(0x0,0x1) // FMUV5X I2C2 BMP388,             Rev 1
+#define V5X02   HW_VER_REV(0x0,0x2) // FMUV5X,                         Rev 2
+#define V5X40   HW_VER_REV(0x4,0x0) // FMUV5X, HB CM4 base             Rev 0
+#define V5X41   HW_VER_REV(0x4,0x1) // FMUV5X I2C2 BMP388, HB CM4 base Rev 1
+#define V5X42   HW_VER_REV(0x4,0x2) // FMUV5X, HB CM4 base             Rev 2
+#define V5X50   HW_VER_REV(0x5,0x0) // FMUV5X, HB Mini                 Rev 0
+#define V5X51   HW_VER_REV(0x5,0x1) // FMUV5X I2C2 BMP388, HB Mini     Rev 1
+#define V5X52   HW_VER_REV(0x5,0x2) // FMUV5X, HB Mini                 Rev 2
+#define V5X90   HW_VER_REV(0x9,0x0) // NO USB,                         Rev 0
+#define V5X91   HW_VER_REV(0x9,0x1) // NO USB I2C2 BMP388,             Rev 1
+#define V5X92   HW_VER_REV(0x9,0x2) // NO USB I2C2 BMP388,             Rev 2
+#define V5Xa0   HW_VER_REV(0xa,0x0) // NO USB (Q),                     Rev 0
+#define V5Xa1   HW_VER_REV(0xa,0x1) // NO USB (Q) I2C2 BMP388,         Rev 1
+#define V5Xa2   HW_VER_REV(0xa,0x2) // NO USB (Q) I2C2 BMP388,         Rev 2
+#define V5X101   HW_VER_REV(0x10,0x1) // NO USB (Q) I2C2 BMP388,        Rev 1
 
 #define UAVCAN_NUM_IFACES_RUNTIME 1
 

boards/px4/fmu-v5x/src/manifest.c

@@ -0,0 +1,222 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manifest.c
+ *
+ * This module supplies the interface to the manifest of hardware that is
+ * optional and dependent on the HW REV and HW VER IDs
+ *
+ * The manifest allows the system to know whether a hardware option
+ * say for example the PX4IO is an no-pop option vs it is broken.
+ *
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <board_config.h>
+
+#include <inttypes.h>
+#include <stdbool.h>
+#include <syslog.h>
+
+#include "systemlib/px4_macros.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+typedef struct {
+	uint32_t                hw_ver_rev; /* the version and revision */
+	const px4_hw_mft_item_t *mft;       /* The first entry */
+	uint32_t                entries;    /* the lenght of the list */
+} px4_hw_mft_list_entry_t;
+
+typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
+#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1
+
+static const px4_hw_mft_item_t device_unsupported = {0, 0, 0};
+
+// List of components on a specific board configuration
+// The index of those components is given by the enum (px4_hw_mft_item_id_t)
+// declared in board_common.h
+static const px4_hw_mft_item_t hw_mft_list_v0500[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0550[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0510[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0509[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static px4_hw_mft_list_entry_t mft_lists[] = {
+//  ver_rev
+	{V5X00, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,                 Rev 0
+	{V5X01, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,                 Rev 1
+	{V5X02, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,                 Rev 2
+	{V5X10, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)}, // NO PX4IO,               Rev 0
+	{V5X41, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,  HB CM4 base    Rev 1
+	{V5X42, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,  HB CM4 base    Rev 2
+	{V5X51, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X,   HB Mini       Rev 1
+	{V5X52, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X,   HB Mini       Rev 2
+	{V5X90, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB,                 Rev 0
+	{V5X91, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 1
+	{V5X92, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 2
+	{V5Xa0, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q),             Rev 0
+	{V5Xa1, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 1
+	{V5Xa2, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 2
+	{V5X101, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 1
+};
+
+/************************************************************************************
+ * Name: board_query_manifest
+ *
+ * Description:
+ *   Optional returns manifest item.
+ *
+ * Input Parameters:
+ *   manifest_id - the ID for the manifest item to retrieve
+ *
+ * Returned Value:
+ *   0 - item is not in manifest => assume legacy operations
+ *   pointer to a manifest item
+ *
+ ************************************************************************************/
+
+__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
+{
+	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
+
+	if (boards_manifest == px4_hw_mft_list_uninitialized) {
+		uint32_t ver_rev = board_get_hw_version() << 16;
+		ver_rev |= board_get_hw_revision();
+
+		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
+			if (mft_lists[i].hw_ver_rev == ver_rev) {
+				boards_manifest = &mft_lists[i];
+				break;
+			}
+		}
+
+		if (boards_manifest == px4_hw_mft_list_uninitialized) {
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
+		}
+	}
+
+	px4_hw_mft_item rv = &device_unsupported;
+
+	if (boards_manifest != px4_hw_mft_list_uninitialized &&
+	    id < boards_manifest->entries) {
+		rv = &boards_manifest->mft[id];
+	}
+
+	return rv;
+}

boards/px4/fmu-v5x/src/mtd.cpp

@@ -31,9 +31,6 @@
  *
  ****************************************************************************/
 
-#include <nuttx/config.h>
-#include <board_config.h>
-
 #include <nuttx/spi/spi.h>
 #include <px4_platform_common/px4_manifest.h>
 //                                                              KiB BS    nB
@@ -123,16 +120,10 @@ static const px4_mft_entry_s mtd_mft = {
 	.pmft = (void *) &board_mtd_config,
 };
 
-static const px4_mft_entry_s mft_mft = {
-	.type = MFT,
-	.pmft = (void *) system_query_manifest,
-};
-
 static const px4_mft_s mft = {
-	.nmft = 2,
+	.nmft = 1,
 	.mfts = {
-		&mtd_mft,
-		&mft_mft,
+		&mtd_mft
 	}
 };
 

boards/px4/fmu-v5x/src/spi.cpp

@@ -36,7 +36,7 @@
 #include <nuttx/spi/spi.h>
 
 constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
-	initSPIFmumID(V5X_0, {
+	initSPIHWVersion(V5X00, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
 		}, {GPIO::PortI, GPIO::Pin11}),
@@ -60,7 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),
 
-	initSPIFmumID(V5X_1, {
+	initSPIHWVersion(V5X01, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
 		}, {GPIO::PortI, GPIO::Pin11}),
@@ -84,7 +84,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),
 
-	initSPIFmumID(V5X_2, {
+	initSPIHWVersion(V5X02, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
 		}, {GPIO::PortI, GPIO::Pin11}),
@@ -107,6 +107,99 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),
 
+	initSPIHWVersion(V5X41, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		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}),
+		}, {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(V5X42, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		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(V5X51, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		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}),
+		}, {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(V5X52, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		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}),
+		}),
+	}),
 };
 
 static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);

boards/px4/fmu-v6c/init/rc.board_defaults

@@ -9,3 +9,5 @@ param set-default BAT2_V_DIV 18.1
 
 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152
+
+param set-default SYS_USE_IO 1

boards/px4/fmu-v6x/default.px4board

@@ -20,25 +20,22 @@ 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
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y

boards/px4/fmu-v6x/init/rc.board_defaults

@@ -16,5 +16,6 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1
 
+param set-default SYS_USE_IO 1
 
 safety_button start

boards/px4/fmu-v6x/init/rc.board_sensors

@@ -4,7 +4,7 @@
 #------------------------------------------------------------------------------
 set HAVE_PM2 yes
 
-if mft query -q -k MFT -s MFT_PM2 -v 0
+if ver hwtypecmp V6X005000 V6X005001 V6X005003 V6X005004
 then
 	set HAVE_PM2 no
 fi
@@ -48,74 +48,57 @@ then
 fi
 
 
-# Keep nesting shallow
-if ver hwtypecmp V6X006 V6X008
+if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004
 then
-	if ver hwtypecmp V6X006
-	then
-		# Internal SPI bus ICM45686
-		adis16470 -s -R 0 start
-		iim42652 -s -R 6 start
-		icm45686 -s -R 10 start
-	else
-		# Internal SPI bus 3x ICM45686
-		icm45686 -b 3 -s -R 0 start
-		icm45686 -b 2 -s -R 0 start
-		icm45686 -b 1 -s -R 10 start
-	fi
+	# Internal SPI bus ICM20649
+	icm20649 -s -R 6 start
 else
-	if ver hwtypecmp V6X004
+	# Internal SPI BMI088
+	if ver hwtypecmp V6X009010 V6X010010
 	then
-		# Internal SPI bus ICM20649
-		icm20649 -s -R 6 start
+		bmi088 -A -R 6 -s start
+		bmi088 -G -R 6 -s start
 	else
-		# Internal SPI BMI088
-		if ver hwbasecmp 009 010
+		if ver hwtypecmp V6X000010
 		then
-			bmi088 -A -R 6 -s start
-			bmi088 -G -R 6 -s start
+			bmi088 -A -R 0 -s start
+			bmi088 -G -R 0 -s start
 		else
-			if ver hwtypecmp V6X010
-			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
-
-	# Internal SPI bus ICM42688p
-	if ver hwbasecmp 009 010
-	then
-		icm42688p -R 12 -s start
-	else
-		if ver hwtypecmp V6X010
-		then
-			icm42688p -R 14 -s start
-		else
-			icm42688p -R 6 -s start
-		fi
-	fi
-
-	if ver hwtypecmp V6X003 V6X004
-	then
-		# Internal SPI bus ICM-42670-P (hard-mounted)
-		icm42670p -R 10 -s start
-	else
-		if ver hwbasecmp 009 010
-		then
-			icm20602 -R 6 -s start
-		else
-			# Internal SPI bus ICM-20649 (hard-mounted)
-			icm20649 -R 14 -s start
+			bmi088 -A -R 4 -s start
+			bmi088 -G -R 4 -s start
 		fi
 	fi
 fi
 
+# Internal SPI bus ICM42688p
+if ver hwtypecmp V6X009010 V6X010010
+then
+	icm42688p -R 12 -s start
+else
+	if ver hwtypecmp V6X000010
+	then
+		icm42688p -R 14 -s start
+	else
+		icm42688p -R 6 -s start
+	fi
+fi
+
+if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004
+then
+	# Internal SPI bus ICM-42670-P (hard-mounted)
+	icm42670p -R 10 -s start
+else
+	if ver hwtypecmp V6X009010 V6X010010
+	then
+		icm20602 -R 6 -s start
+	else
+		# Internal SPI bus ICM-20649 (hard-mounted)
+		icm20649 -R 14 -s start
+	fi
+fi
+
 # Internal magnetometer on I2c
-if ver hwtypecmp V6X001
+if ver hwtypecmp V6X002001
 then
 	rm3100 -I -b 4 start
 else
@@ -129,7 +112,7 @@ ist8310 -X -b 1 -R 10 start
 # Possible internal Baro
 if param compare SENS_INT_BARO_EN 1
 then
-	if ver hwtypecmp V6X001 V6X006 V6X008
+	if ver hwtypecmp V6X002001
 	then
 		icp201xx -I -a 0x64 start
 	else
@@ -138,7 +121,7 @@ then
 fi
 
 #external baro
-if ver hwtypecmp V6X001
+if ver hwtypecmp V6X002001
 then
 	icp201xx -X start
 else

boards/px4/fmu-v6x/nuttx-config/include/board.h

@@ -380,9 +380,7 @@
 #define GPIO_UART5_RX    GPIO_UART5_RX_3    /* PD2  */
 #define GPIO_UART5_TX    GPIO_UART5_TX_3    /* PC12 */
 // GPIO_UART5_RTS   no remap                /* PC8  */
-#undef GPIO_UART5_CTS
-#define GPIO_UART5_CTS   ((GPIO_ALT|GPIO_AF8|GPIO_PORTC|GPIO_PIN9) | GPIO_PULLDOWN) /* PC9  */
-
+// GPIO_UART5_CTS  No remap                 /* PC9  */
 
 #define GPIO_USART6_RX   GPIO_USART6_RX_1   /* PC7 */
 #define GPIO_USART6_TX   GPIO_USART6_TX_1   /* PC6  */

boards/px4/fmu-v6x/src/CMakeLists.txt

@@ -55,6 +55,7 @@ else()
 		init.c
 		led.c
 		mtd.cpp
+		manifest.c
 		sdio.c
 		spi.cpp
 		timer_config.cpp

boards/px4/fmu-v6x/src/board_config.h

@@ -208,22 +208,36 @@
 #define BOARD_ADC_OPEN_CIRCUIT_V     (5.6f)
 
 /* HW Version and Revision drive signals Default to 1 to detect */
-#define BOARD_HAS_HW_SPLIT_VERSIONING
+#define BOARD_HAS_HW_VERSIONING
 
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PH4 */  GPIO_ADC3_INP15
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
 #define HW_INFO_INIT_PREFIX    "V6X"
 
-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 7
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 11 // Rev 0 and Rev 3,4 Sensor sets
 //                 Base/FMUM
-#define V6X_0     HW_FMUM_ID(0x0)   // FMUV6X, Auterion,HB  Sensor Set Rev 0
-#define V6X_1     HW_FMUM_ID(0x1)   // FMUV6X, CUAV Sensor Set Rev 1
-#define V6X_3     HW_FMUM_ID(0x3)   // FMUV6X, HB              Sensor Set Rev 3
-#define V6X_4     HW_FMUM_ID(0x4)   // FMUV6X, HB              Sensor Set Rev 4
-#define V6X_6     HW_FMUM_ID(0x6)   // FMUV6X, HB              Sensor Set Rev 6
-#define V6X_8     HW_FMUM_ID(0x8)   // FMUV6X, HB              Sensor Set Rev 8
-#define V6X_16    HW_FMUM_ID(0x10)  // FMUV6X, Auterion        Sensor Set Rev 16 from EEPROM
+#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 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 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 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 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
+
+
 
 #define UAVCAN_NUM_IFACES_RUNTIME  1
 

boards/px4/fmu-v6x/src/manifest.c

@@ -0,0 +1,205 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manifest.c
+ *
+ * This module supplies the interface to the manifest of hardware that is
+ * optional and dependent on the HW REV and HW VER IDs
+ *
+ * The manifest allows the system to know whether a hardware option
+ * say for example the PX4IO is an no-pop option vs it is broken.
+ *
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <board_config.h>
+
+#include <inttypes.h>
+#include <stdbool.h>
+#include <syslog.h>
+
+#include "systemlib/px4_macros.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+typedef struct {
+	uint32_t                hw_ver_rev; /* the version and revision */
+	const px4_hw_mft_item_t *mft;       /* The first entry */
+	uint32_t                entries;    /* the lenght of the list */
+} px4_hw_mft_list_entry_t;
+
+typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
+#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1
+
+static const px4_hw_mft_item_t device_unsupported = {0, 0, 0};
+
+// List of components on a specific board configuration
+// The index of those components is given by the enum (px4_hw_mft_item_id_t)
+// declared in board_common.h
+static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0650[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+};
+
+
+static px4_hw_mft_list_entry_t mft_lists[] = {
+//  ver_rev
+	{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
+	{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
+	{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
+	{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
+	{V6X1010, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X,     rev from EEPROM     Auterion Skynode ver10
+};
+
+/************************************************************************************
+ * Name: board_query_manifest
+ *
+ * Description:
+ *   Optional returns manifest item.
+ *
+ * Input Parameters:
+ *   manifest_id - the ID for the manifest item to retrieve
+ *
+ * Returned Value:
+ *   0 - item is not in manifest => assume legacy operations
+ *   pointer to a manifest item
+ *
+ ************************************************************************************/
+
+__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
+{
+	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
+
+	if (boards_manifest == px4_hw_mft_list_uninitialized) {
+		uint32_t ver_rev = board_get_hw_version() << 16;
+		ver_rev |= board_get_hw_revision();
+
+		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
+			if (mft_lists[i].hw_ver_rev == ver_rev) {
+				boards_manifest = &mft_lists[i];
+				break;
+			}
+		}
+
+		if (boards_manifest == px4_hw_mft_list_uninitialized) {
+			syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev);
+		}
+	}
+
+	px4_hw_mft_item rv = &device_unsupported;
+
+	if (boards_manifest != px4_hw_mft_list_uninitialized &&
+	    id < boards_manifest->entries) {
+		rv = &boards_manifest->mft[id];
+	}
+
+	return rv;
+}

boards/px4/fmu-v6x/src/mtd.cpp

@@ -31,9 +31,6 @@
  *
  ****************************************************************************/
 
-#include <nuttx/config.h>
-#include <board_config.h>
-
 #include <nuttx/spi/spi.h>
 #include <px4_platform_common/px4_manifest.h>
 //                                                              KiB BS    nB
@@ -123,16 +120,10 @@ static const px4_mft_entry_s mtd_mft = {
 	.pmft = (void *) &board_mtd_config,
 };
 
-static const px4_mft_entry_s mft_mft = {
-	.type = MFT,
-	.pmft = (void *) system_query_manifest,
-};
-
 static const px4_mft_s mft = {
-	.nmft = 2,
+	.nmft = 1,
 	.mfts = {
-		&mtd_mft,
-		&mft_mft,
+		&mtd_mft
 	}
 };
 

boards/px4/fmu-v6x/src/spi.cpp

@@ -36,7 +36,7 @@
 #include <nuttx/spi/spi.h>
 
 constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
-	initSPIFmumID(V6X_0, {
+	initSPIHWVersion(V6X00, {
 		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}),
@@ -60,31 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),
 
-	initSPIFmumID(V6X_1, {
-		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}),
-		}, {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}),
-		}),
-	}),
-
-	initSPIFmumID(V6X_3, {
+	initSPIHWVersion(V6X03, {
 		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}),
@@ -108,7 +84,79 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),
 
-	initSPIFmumID(V6X_4, {
+	initSPIHWVersion(V6X21, {
+		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}),
+		}, {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(V6X43, {
+		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_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(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(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}),
@@ -131,15 +179,39 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 		}),
 	}),
 
-	initSPIFmumID(V6X_6, {
+	// 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_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+			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_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+			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_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			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
@@ -153,16 +225,16 @@ 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}),
 		}),
 	}),
-
-	initSPIFmumID(V6X_8, {
+	initSPIHWVersion(V6X53, {
 		initSPIBus(SPI::Bus::SPI1, {
-			initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+			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_ICM45686, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+			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_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			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
@@ -176,9 +248,52 @@ 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}),
 		}),
 	}),
-
-
-	initSPIFmumID(V6X_16, {
+	initSPIHWVersion(V6X54, {
+		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(V6X0910, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, 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}),
+		}, {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(V6X1010, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
 		}, {GPIO::PortI, GPIO::Pin11}),
@@ -201,7 +316,6 @@ 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}),
 		}),
 	}),
-
 };
 
 static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);

boards/sky-drones/smartap-airlink/init/rc.board_defaults

@@ -25,6 +25,8 @@ param set-default BAT_V_OFFS_CURR 0.413
 # Disable safety switch
 param set-default CBRK_IO_SAFETY 22027
 
+param set-default SYS_USE_IO 1
+
 safety_button start
 
 set LOGGER_BUF 32

boards/sky-drones/smartap-airlink/src/board_config.h

@@ -168,7 +168,7 @@
 
 /* HW Version and Revision drive signals Default to 1 to detect */
 
-#define BOARD_HAS_HW_VERSIONING // migrate to Split
+#define BOARD_HAS_HW_VERSIONING
 
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PF5 */  ADC3_GPIO(15)

boards/thepeach/k1/init/rc.board_defaults

@@ -5,3 +5,5 @@
 
 param set-default BAT1_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
+
+param set-default SYS_USE_IO 1

boards/thepeach/r1/init/rc.board_defaults

@@ -5,3 +5,5 @@
 
 param set-default BAT1_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
+
+param set-default SYS_USE_IO 1

platforms/common/CMakeLists.txt

@@ -52,7 +52,6 @@ add_library(px4_platform STATIC
 	px4_cli.cpp
 	shutdown.cpp
 	spi.cpp
-	pab_manifest.c
 	${SRCS}
 )
 target_link_libraries(px4_platform prebuild_targets px4_work_queue)

platforms/common/include/px4_platform_common/board_common.h

@@ -265,18 +265,6 @@
 #  define HW_VER_REV(v,r)       ((uint32_t)((v) & 0xffff) << 16) | ((uint32_t)(r) & 0xffff)
 #endif
 
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-typedef uint16_t hw_fmun_id_t;
-typedef uint16_t hw_base_id_t;
-// Original Signals GPIO_HW_REV_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the FMUM
-// Original Signals GPIO_HW_VER_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the BASE
-#  define BOARD_HAS_VERSIONING 1
-#  define HW_FMUM_ID(rev)       ((hw_fmun_id_t)(rev) & 0xffff)
-#  define HW_BASE_ID(ver)       ((hw_base_id_t)(ver) & 0xffff)
-#  define GET_HW_FMUM_ID()      (HW_FMUM_ID(board_get_hw_revision()))
-#  define GET_HW_BASE_ID()      (HW_BASE_ID(board_get_hw_version()))
-#endif
-
 #define HW_INFO_REV_DIGITS    3
 #define HW_INFO_VER_DIGITS    3
 
@@ -666,51 +654,20 @@ bool board_booted_by_px4(void);
  ************************************************************************************/
 
 typedef enum {
-	PX4_MFT_PX4IO      = 0,
-	PX4_MFT_USB        = 1,
-	PX4_MFT_CAN2       = 2,
-	PX4_MFT_CAN3       = 3,
-	PX4_MFT_PM2        = 4,
-	PX4_MFT_ETHERNET   = 5,
-	PX4_MFT_T1_ETH     = 6,
-	PX4_MFT_T100_ETH   = 7,
-	PX4_MFT_T1000_ETH  = 8,
+	PX4_MFT_PX4IO = 0,
+	PX4_MFT_USB   = 1,
+	PX4_MFT_CAN2  = 2,
+	PX4_MFT_CAN3  = 3,
 } px4_hw_mft_item_id_t;
 
-typedef int (*system_query_func_t)(const char *sub,  const char *val, void *out);
-
-#define PX4_MFT_MFT_TYPES  { \
-		PX4_MFT_PX4IO,           \
-		PX4_MFT_USB,             \
-		PX4_MFT_CAN2,            \
-		PX4_MFT_CAN3,            \
-		PX4_MFT_PM2,             \
-		PX4_MFT_ETHERNET,        \
-		PX4_MFT_T1_ETH,          \
-		PX4_MFT_T100_ETH,        \
-		PX4_MFT_T1000_ETH }
-
-#define PX4_MFT_MFT_STR_TYPES  { \
-		"MFT_PX4IO",             \
-		"MFT_USB",               \
-		"MFT_CAN2",              \
-		"MFT_CAN3",              \
-		"MFT_PM2",               \
-		"MFT_ETHERNET",          \
-		"MFT_T1_ETH",            \
-		"MFT_T100_ETH",          \
-		"MFT_T1000_ETH",         \
-		"MFT_T1000_ETH"}
-
 typedef enum {
-	px4_hw_con_unknown   = 0,
-	px4_hw_con_onboard   = 1,
+	px4_hw_con_unknown  = 0,
+	px4_hw_con_onboard  = 1,
 	px4_hw_con_connector = 3,
 } px4_hw_connection_t;
 
 
 typedef struct {
-	unsigned int id:         16;  /* The id px4_hw_mft_item_id_t */
 	unsigned int present:    1;   /* 1 if this board have this item */
 	unsigned int mandatory:  1;   /* 1 if this item has to be present and working */
 	unsigned int connection: 2;   /* See px4_hw_connection_t */
@@ -722,7 +679,7 @@ typedef const px4_hw_mft_item_t  *px4_hw_mft_item;
 
 #if defined(BOARD_HAS_VERSIONING)
 __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id);
-__EXPORT int system_query_manifest(const char *sub,  const char *val, void *out);
+
 #  define PX4_MFT_HW_SUPPORTED(ID)           (board_query_manifest((ID))->present)
 #  define PX4_MFT_HW_REQUIRED(ID)            (board_query_manifest((ID))->mandatory)
 #  define PX4_MFT_HW_IS_ONBOARD(ID)          (board_query_manifest((ID))->connection == px4_hw_con_onboard)
@@ -795,26 +752,6 @@ __EXPORT const char *board_get_hw_type_name(void);
 #define board_get_hw_type_name() ""
 #endif
 
-/************************************************************************************
- * Name: board_get_hw_base_type_name
- *
- * Description:
- *   Optional returns a 0 terminated string defining the HW type.
- *
- * Input Parameters:
- *   None
- *
- * Returned Value:
- *   a 0 terminated string defining the HW type. This may be a 0 length string ""
- *
- ************************************************************************************/
-
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-__EXPORT const char *board_get_hw_base_type_name(void);
-#else
-#define board_get_hw_base_type_name() ""
-#endif
-
 /************************************************************************************
  * Name: board_get_hw_version
  *

platforms/common/include/px4_platform_common/spi.h

@@ -73,11 +73,7 @@ struct px4_spi_bus_t {
 
 struct px4_spi_bus_all_hw_t {
 	px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS];
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-	hw_fmun_id_t board_hw_fmun_id {USHRT_MAX};
-#else
-	int board_hw_version_revision {-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused
-#endif
+	int board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused
 };
 
 #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1

platforms/common/pab_manifest.c

@@ -1,404 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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.
- *
- ****************************************************************************/
-
-/**
- * @file pab_manifest.c
- *
- * This module supplies the interface to the manifest of hardware that is
- * optional and dependent on the HW_BASE_ID
- *
- * The manifest allows the system to know whether a hardware option
- * say for example the PX4IO is an no-pop option vs it is broken.
- *
- */
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <board_config.h>
-
-#include <inttypes.h>
-#include <stdbool.h>
-#include <syslog.h>
-
-#include "systemlib/px4_macros.h"
-
-/****************************************************************************
- * Pre-Processor Definitions
- ****************************************************************************/
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-
-typedef struct {
-	hw_base_id_t             hw_base_id; /* The ID of the Base */
-	const px4_hw_mft_item_t *mft;        /* The first entry */
-	uint32_t                 entries;    /* the lenght of the list */
-} px4_hw_mft_list_entry_t;
-
-typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
-#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1
-
-static const px4_hw_mft_item_t device_unsupported = {0, 0, 0};
-
-// List of components on a specific base board configuration
-// The ids of those components is given by the enum (px4_hw_mft_item_id_t)
-// declared in board_common.h
-
-// BASE ID 0 Auterion vXx base board
-static const px4_hw_mft_item_t base_configuration_0[] = {
-	{
-		.id          =  PX4_MFT_PX4IO,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_USB,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_CAN2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_CAN3,
-		.present     = 0,
-		.mandatory   = 0,
-		.connection  = px4_hw_con_unknown,
-	},
-	{
-		.id          =  PX4_MFT_PM2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_ETHERNET,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_connector,
-	},
-	{
-		.id          = PX4_MFT_T100_ETH,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_connector,
-	},
-};
-
-// BASE ID 1 vXx base without px4io
-static const px4_hw_mft_item_t base_configuration_1[] = {
-	{
-		.id          = PX4_MFT_PX4IO,
-		.present     = 0,
-		.mandatory   = 0,
-		.connection  = px4_hw_con_unknown,
-	},
-	{
-		.id          = PX4_MFT_USB,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_CAN2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_CAN3,
-		.present     = 0,
-		.mandatory   = 0,
-		.connection  = px4_hw_con_unknown,
-	},
-	{
-		.id          =  PX4_MFT_PM2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_ETHERNET,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_connector,
-	},
-	{
-		.id          = PX4_MFT_T100_ETH,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_connector,
-	},
-};
-
-// BASE ID 2 Modal AI Alaised to ID 0
-
-// BASE ID 3 NXP T1 PHY
-static const px4_hw_mft_item_t base_configuration_3[] = {
-	{
-		.id          = PX4_MFT_PX4IO,
-		.present     = 0,
-		.mandatory   = 0,
-		.connection  = px4_hw_con_unknown,
-	},
-	{
-		.id          = PX4_MFT_USB,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_CAN2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_CAN3,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          =  PX4_MFT_PM2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_ETHERNET,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_connector,
-	},
-	{
-		.id          = PX4_MFT_T1_ETH,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_connector,
-	},
-};
-
-// BASE ID 4 HB CM4 Alaised to ID 0
-
-// BASE ID 5  HB min
-static const px4_hw_mft_item_t base_configuration_5[] = {
-	{
-		.id          = PX4_MFT_PX4IO,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_USB,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_CAN2,
-		.present     = 0,
-		.mandatory   = 0,
-		.connection  = px4_hw_con_unknown,
-	},
-	{
-		.id          = PX4_MFT_CAN3,
-		.present     = 0,
-		.mandatory   = 0,
-		.connection  = px4_hw_con_unknown,
-	},
-	{
-		.id          = PX4_MFT_ETHERNET,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_connector,
-	},
-	{
-		.id          = PX4_MFT_T100_ETH,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_connector,
-	},
-};
-
-// BASE ID 6  Not allocated
-// BASE ID 7  Read from EEPROM
-// BASE ID 8  Skynode QS with USB - Alaised to ID 0
-
-// BASE ID 9  Auterion Skynode base RC9 & older (no usb
-static const px4_hw_mft_item_t base_configuration_9[] = {
-	{
-		.id          =  PX4_MFT_PX4IO,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_USB,
-		.present     = 0,
-		.mandatory   = 0,
-		.connection  = px4_hw_con_unknown,
-	},
-	{
-		.id          = PX4_MFT_CAN2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_CAN3,
-		.present     = 0,
-		.mandatory   = 0,
-		.connection  = px4_hw_con_unknown,
-	},
-	{
-		.id          =  PX4_MFT_PM2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          =  PX4_MFT_PM2,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-	{
-		.id          = PX4_MFT_ETHERNET,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_connector,
-	},
-	{
-		.id          = PX4_MFT_T100_ETH,
-		.present     = 1,
-		.mandatory   = 1,
-		.connection  = px4_hw_con_onboard,
-	},
-};
-
-// BASE ID 10  Skynode QS no USB Alaised to ID 9
-// BASE ID 16  Auterion Skynode RC10, RC11, RC12, RC13 Alaised to ID 0
-
-static px4_hw_mft_list_entry_t mft_lists[] = {
-//  ver_rev
-	{HW_BASE_ID(0),      base_configuration_0, arraySize(base_configuration_0)},   // std Base with PX4IO
-	{HW_BASE_ID(1),      base_configuration_1, arraySize(base_configuration_1)},   // std Base No PX4IO
-	{HW_BASE_ID(2),      base_configuration_0, arraySize(base_configuration_0)},   // CUAV Base
-	{HW_BASE_ID(3),      base_configuration_3, arraySize(base_configuration_3)},   // NXP T1 PHY
-	{HW_BASE_ID(4),      base_configuration_0, arraySize(base_configuration_0)},   // HB CM4 base
-	{HW_BASE_ID(5),      base_configuration_5, arraySize(base_configuration_5)},   // HB Mini
-	{HW_BASE_ID(8),      base_configuration_0, arraySize(base_configuration_0)},   // Auterion Skynode ver 8 Aliased to 0
-	{HW_BASE_ID(9),      base_configuration_9, arraySize(base_configuration_9)},   // Auterion Skynode ver 9
-	{HW_BASE_ID(10),     base_configuration_9, arraySize(base_configuration_9)},   // Auterion Skynode ver 10
-	{HW_BASE_ID(16),     base_configuration_0, arraySize(base_configuration_0)},   // Auterion Skynode ver 16
-	{HW_BASE_ID(0x100),  base_configuration_0, arraySize(base_configuration_0)},   // Holybro Pixhawk Jetson Baseboard ver 0x100 Alaised to ID 0
-};
-
-/************************************************************************************
- * Name: base_query_manifest
- *
- * Description:
- *   Optional returns manifest item.
- *
- * Input Parameters:
- *   manifest_id - the ID for the manifest item to retrieve
- *
- * Returned Value:
- *   0 - item is not in manifest => assume legacy operations
- *   pointer to a manifest item
- *
- ************************************************************************************/
-
-__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
-{
-	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
-
-	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		hw_base_id_t hw_base_id = GET_HW_BASE_ID();
-
-		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
-			if (mft_lists[i].hw_base_id == hw_base_id) {
-				boards_manifest = &mft_lists[i];
-				break;
-			}
-		}
-
-		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %04" PRIx16 " is not supported!\n", hw_base_id);
-		}
-	}
-
-	px4_hw_mft_item rv = &device_unsupported;
-
-	if (boards_manifest != px4_hw_mft_list_uninitialized)
-		for (unsigned int ndx = 0; ndx < boards_manifest->entries; ndx++) {
-			if (boards_manifest->mft[ndx].id == id) {
-				rv = &boards_manifest->mft[id];
-				break;
-			}
-		}
-
-	return rv;
-}
-
-__EXPORT int system_query_manifest(const char *sub,  const char *val, void *out)
-{
-	static const char *keys[] = PX4_MFT_MFT_STR_TYPES;
-	static const px4_hw_mft_item_id_t item_ids[] = PX4_MFT_MFT_TYPES;
-	px4_hw_mft_item rv = &device_unsupported;
-	int id = -1;
-	int intval = atoi(val);
-
-	for (unsigned int k = 0; k < arraySize(keys); k++) {
-		if (!strcmp(keys[k], sub)) {
-			id = item_ids[k];
-			break;
-		}
-	}
-
-	if (id != -1) {
-		// In case we have to filter when a FMUM is mounted to a BASE
-		// For now just use the board
-		rv = board_query_manifest(id);
-		return rv->present == intval ? OK : -ENXIO;
-	}
-
-	return -ENOENT;
-}
-#endif

platforms/common/spi.cpp

@@ -44,26 +44,12 @@
 #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
 void px4_set_spi_buses_from_hw_version()
 {
-#  if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-	int hw_fmun_id = GET_HW_FMUM_ID();
-
-	for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) {
-		if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_fmun_id == 0) {
-			px4_spi_buses = px4_spi_buses_all_hw[i].buses;
-		}
-
-		if (px4_spi_buses_all_hw[i].board_hw_fmun_id == hw_fmun_id) {
-			px4_spi_buses = px4_spi_buses_all_hw[i].buses;
-		}
-	}
-
-#  else
-
-#    if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
+#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
 	int hw_version_revision = board_get_hw_version();
-#    else
+#else
 	int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision());
-#    endif
+#endif
+
 
 	for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) {
 		if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version_revision == 0) {
@@ -75,8 +61,6 @@ void px4_set_spi_buses_from_hw_version()
 		}
 	}
 
-#  endif
-
 	if (!px4_spi_buses) { // fallback
 		px4_spi_buses = px4_spi_buses_all_hw[0].buses;
 	}

platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h

@@ -36,10 +36,6 @@
 __BEGIN_DECLS
 
 #define HW_INFO_SUFFIX        "%0" STRINGIFY(HW_INFO_VER_DIGITS) "x%0" STRINGIFY(HW_INFO_REV_DIGITS) "x"
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-#  define HW_INFO_FMUM_SUFFIX   "%0" STRINGIFY(HW_INFO_REV_DIGITS) "x"
-#  define HW_INFO_BASE_SUFFIX   "%0" STRINGIFY(HW_INFO_VER_DIGITS) "x"
-#endif
 /************************************************************************************
   * Name: board_determine_hw_info
  *

platforms/nuttx/src/px4/common/px4_manifest.cpp

@@ -104,13 +104,6 @@ __EXPORT int px4_mft_query(const px4_mft_s *mft, px4_manifest_types_e type,
 					break;
 
 				case MFT:
-					if (mft->mfts[m]->pmft != nullptr) {
-						system_query_func_t query = (system_query_func_t) mft->mfts[m]->pmft;
-						return query(sub, val, nullptr);
-					}
-
-					break;
-
 				default:
 					rv = -ENODATA;
 					break;

platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c

@@ -51,7 +51,7 @@
 #include <lib/crc/crc.h>
 #include <lib/systemlib/px4_macros.h>
 
-#if defined(BOARD_HAS_HW_VERSIONING) || defined(BOARD_HAS_HW_SPLIT_VERSIONING)
+#if defined(BOARD_HAS_HW_VERSIONING)
 
 #  if defined(GPIO_HW_VER_REV_DRIVE)
 #    define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE
@@ -67,9 +67,7 @@
 static int hw_version = 0;
 static int hw_revision = 0;
 static char hw_info[HW_INFO_SIZE] = {0};
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-static char hw_base_info[HW_INFO_SIZE] = {0};
-#endif
+
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
@@ -368,27 +366,6 @@ __EXPORT const char *board_get_hw_type_name()
 	return (const char *) hw_info;
 }
 
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-/************************************************************************************
- * Name: board_get_hw_base_type_name
- *
- * Description:
- *   Optional returns a 0 terminated string defining the base type.
- *
- * Input Parameters:
- *   None
- *
- * Returned Value:
- *   a 0 terminated string defining the HW type. This my be a 0 length string ""
- *
- ************************************************************************************/
-
-__EXPORT const char *board_get_hw_base_type_name()
-{
-	return (const char *) hw_base_info;
-}
-#endif
-
 /************************************************************************************
  * Name: board_get_hw_version
  *
@@ -490,12 +467,7 @@ int board_determine_hw_info()
 	}
 
 	if (rv == OK) {
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-		snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_FMUM_SUFFIX, GET_HW_FMUM_ID());
-		snprintf(hw_base_info, sizeof(hw_info), HW_INFO_BASE_SUFFIX, GET_HW_BASE_ID());
-#else
 		snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision);
-#endif
 	}
 
 	return rv;

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h

@@ -136,21 +136,6 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI
 struct px4_spi_bus_array_t {
 	px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS];
 };
-
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-static inline constexpr px4_spi_bus_all_hw_t initSPIFmumID(hw_fmun_id_t hw_fmun_id,
-		const px4_spi_bus_array_t &bus_items)
-{
-	px4_spi_bus_all_hw_t ret{};
-
-	for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
-		ret.buses[i] = bus_items.item[i];
-	}
-
-	ret.board_hw_fmun_id = hw_fmun_id;
-	return ret;
-}
-#else
 static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision,
 		const px4_spi_bus_array_t &bus_items)
 {
@@ -163,7 +148,6 @@ static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_rev
 	ret.board_hw_version_revision = hw_version_revision;
 	return ret;
 }
-#endif
 constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]);
 
 constexpr bool validateSPIConfig(const px4_spi_bus_all_hw_t spi_buses_conf[BOARD_NUM_SPI_CFG_HW_VERSIONS])

src/drivers/gps/devices

@@ -1 +1 @@
-Subproject commit b3ffec3f173a4dcb4d9e604222a30215023e9880
+Subproject commit 99f5960eca66150e33dc277e71a4c99187c27ddc

src/drivers/imu/invensense/icm45686/ICM45686.cpp

@@ -522,7 +522,7 @@ void ICM45686::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DAT
 			accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
 
 		} else {
-			accel.dt = FIFO_SAMPLE_DT;
+			accel.dt = FIFO_TIMESTAMP_SCALING;
 		}
 
 		// 20 bit hires mode
@@ -634,7 +634,7 @@ void ICM45686::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA
 			gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
 
 		} else {
-			gyro.dt = FIFO_SAMPLE_DT;
+			gyro.dt = FIFO_TIMESTAMP_SCALING;
 		}
 
 		// 20 bit hires mode

src/drivers/imu/invensense/icm45686/ICM45686.hpp

@@ -70,7 +70,7 @@ private:
 	void exit_and_cleanup() override;
 
 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 6400.f};     // 6400 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 

src/drivers/imu/invensense/icm45686/icm45686_main.cpp

@@ -38,7 +38,7 @@
 
 void ICM45686::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("icm45686", "driver");
+	PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);

src/drivers/px4io/px4io.cpp

@@ -336,7 +336,8 @@ private:
 		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
 		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
 		(ParamInt<px4::params::SENS_EN_THERMAL>) _param_sens_en_themal,
-		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl
+		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
+		(ParamInt<px4::params::SYS_USE_IO>) _param_sys_use_io
 	)
 };
 
@@ -467,7 +468,7 @@ int PX4IO::init()
 	}
 
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	if (_param_sys_hitl.get() <= 0) {
+	if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
 		_mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL);
 	}
 
@@ -1556,10 +1557,6 @@ int PX4IO::custom_command(int argc, char *argv[])
 {
 	const char *verb = argv[0];
 
-	if (!strcmp(verb, "supported")) {
-		return 0;
-	}
-
 	if (!strcmp(verb, "checkcrc")) {
 		if (is_running()) {
 			PX4_ERR("io must be stopped");
@@ -1765,7 +1762,7 @@ Output driver communicating with the IO co-processor.
 extern "C" __EXPORT int px4io_main(int argc, char *argv[])
 {
 	if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) {
-		PX4_INFO("PX4IO Not Supported");
+		PX4_ERR("PX4IO Not Supported");
 		return -1;
 	}
 	return PX4IO::main(argc, argv);

src/drivers/px4io/px4io_params.c

@@ -42,6 +42,18 @@
 #include <px4_platform_common/px4_config.h>
 #include <parameters/param.h>
 
+/**
+ * Set usage of IO board
+ *
+ * Can be used to use a configure the use of the IO board.
+ *
+ * @value 0 IO PWM disabled (RC only)
+ * @value 1 IO enabled (RC & PWM)
+ * @reboot_required true
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_USE_IO, 0);
+
 /**
  * S.BUS out
  *

src/lib/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2017-2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2017 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
@@ -61,7 +61,6 @@ add_subdirectory(pid EXCLUDE_FROM_ALL)
 add_subdirectory(pid_design EXCLUDE_FROM_ALL)
 add_subdirectory(rate_control EXCLUDE_FROM_ALL)
 add_subdirectory(rc EXCLUDE_FROM_ALL)
-add_subdirectory(ringbuffer EXCLUDE_FROM_ALL)
 add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL)
 add_subdirectory(slew_rate EXCLUDE_FROM_ALL)
 add_subdirectory(systemlib EXCLUDE_FROM_ALL)
@@ -71,7 +70,6 @@ add_subdirectory(terrain_estimation EXCLUDE_FROM_ALL)
 add_subdirectory(timesync EXCLUDE_FROM_ALL)
 add_subdirectory(tinybson EXCLUDE_FROM_ALL)
 add_subdirectory(tunes EXCLUDE_FROM_ALL)
-add_subdirectory(variable_length_ringbuffer EXCLUDE_FROM_ALL)
 add_subdirectory(version EXCLUDE_FROM_ALL)
 add_subdirectory(weather_vane EXCLUDE_FROM_ALL)
 add_subdirectory(wind_estimator EXCLUDE_FROM_ALL)

src/lib/matrix/matrix/Slice.hpp

@@ -35,8 +35,6 @@ public:
 		assert(y0 + Q <= N);
 	}
 
-	Slice(const Slice<Type, P, Q, M, N> &other) = default;
-
 	const Type &operator()(size_t i, size_t j) const
 	{
 		assert(i < P);
@@ -54,12 +52,6 @@ public:
 		return (*_data)(_x0 + i, _y0 + j);
 	}
 
-	// Separate function needed otherwise the default copy constructor matches before the deep copy implementation
-	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, M, N> &other)
-	{
-		return this->operator=<M, N>(other);
-	}
-
 	template<size_t MM, size_t NN>
 	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, MM, NN> &other)
 	{

src/lib/matrix/test/MatrixSliceTest.cpp

@@ -262,12 +262,3 @@ TEST(MatrixSliceTest, Slice)
 	float O_check_data_12 [4] = {2.5, 3, 4, 5};
 	EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
 }
-
-TEST(MatrixSliceTest, XYAssignmentTest)
-{
-	Vector3f a(1, 2, 3);
-	Vector3f b(4, 5, 6);
-	// Assign first two elements from b to first two slot of a
-	a.xy() = b.xy();
-	EXPECT_EQ(a, Vector3f(4, 5, 3));
-}

src/lib/mixer_module/functions/FunctionActuatorSet.hpp

@@ -61,17 +61,12 @@ public:
 				int index = (int)(vehicle_command.param7 + 0.5f);
 
 				if (index == 0) {
-					if (PX4_ISFINITE(vehicle_command.param1)) { _data[0] = vehicle_command.param1; }
-
-					if (PX4_ISFINITE(vehicle_command.param2)) {_data[1] = vehicle_command.param2; }
-
-					if (PX4_ISFINITE(vehicle_command.param3)) {_data[2] = vehicle_command.param3; }
-
-					if (PX4_ISFINITE(vehicle_command.param4)) {_data[3] = vehicle_command.param4; }
-
-					if (PX4_ISFINITE(vehicle_command.param5)) {_data[4] = vehicle_command.param5; }
-
-					if (PX4_ISFINITE(vehicle_command.param6)) {_data[5] = vehicle_command.param6; }
+					_data[0] = vehicle_command.param1;
+					_data[1] = vehicle_command.param2;
+					_data[2] = vehicle_command.param3;
+					_data[3] = vehicle_command.param4;
+					_data[4] = vehicle_command.param5;
+					_data[5] = vehicle_command.param6;
 				}
 			}
 		}

src/lib/motion_planning/PositionSmoothing.cpp

@@ -134,20 +134,21 @@ const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, co
 	}
 
 	// Get the crossing point using L1-style guidance
-	return _getL1Point(position, waypoints);
+	auto l1_point = _getL1Point(position, waypoints);
+	return {l1_point(0), l1_point(1), target(2)};
 }
 
-const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const
+const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const
 {
-	const Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(),
-				_trajectory[2].getCurrentPosition());
-	const Vector3f u_prev_to_target = (waypoints[1] - waypoints[0]).unit_or_zero();
-	const Vector3f prev_to_pos(pos_traj - waypoints[0]);
-	const Vector3f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
-	const Vector3f closest_pt = waypoints[0] + prev_to_closest;
+	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
+				_trajectory[1].getCurrentPosition());
+	const Vector2f u_prev_to_target = Vector2f(waypoints[1] - waypoints[0]).unit_or_zero();
+	const Vector2f prev_to_pos(pos_traj - Vector2f(waypoints[0]));
+	const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
+	const Vector2f closest_pt = Vector2f(waypoints[0]) + prev_to_closest;
 
 	// Compute along-track error using L1 distance and cross-track error
-	const float crosstrack_error = (closest_pt - pos_traj).length();
+	const float crosstrack_error = Vector2f(closest_pt - pos_traj).length();
 
 	const float l1 = math::max(_target_acceptance_radius, 5.f);
 	float alongtrack_error = 0.f;
@@ -158,7 +159,9 @@ const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve
 	}
 
 	// Position of the point on the line where L1 intersect the line between the two waypoints
-	return closest_pt + alongtrack_error * u_prev_to_target;
+	const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target;
+
+	return l1_point;
 }
 
 const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],

src/lib/motion_planning/PositionSmoothing.hpp

@@ -438,7 +438,7 @@ private:
 	const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
 			bool is_single_waypoint,
 			const Vector3f &feedforward_velocity_setpoint);
-	const Vector3f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
+	const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
 	const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
 	float _getMaxXYSpeed(const Vector3f(&waypoints)[3]) const;
 	float _getMaxZSpeed(const Vector3f(&waypoints)[3]) const;

src/lib/parameters/param_translation.cpp

@@ -210,6 +210,8 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	return false;
+
 	//2023-02-08: translate L1 parameters after removing l1 control
 	{
 		if (strcmp("RWTO_L1_PERIOD", node->name) == 0) {
@@ -230,6 +232,4 @@ bool param_modify_on_import(bson_node_t node)
 			return true;
 		}
 	}
-
-	return false;
 }

src/lib/ringbuffer/CMakeLists.txt

@@ -1,40 +0,0 @@
-############################################################################
-#
-#   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_library(ringbuffer
-	Ringbuffer.cpp
-)
-
-target_include_directories(ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-px4_add_unit_gtest(SRC RingbufferTest.cpp LINKLIBS ringbuffer)

src/lib/ringbuffer/Ringbuffer.cpp

@@ -1,180 +0,0 @@
-/****************************************************************************
- *
- *   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 "Ringbuffer.hpp"
-
-#include <mathlib/mathlib.h>
-#include <assert.h>
-#include <string.h>
-
-
-Ringbuffer::~Ringbuffer()
-{
-	deallocate();
-}
-
-bool Ringbuffer::allocate(size_t buffer_size)
-{
-	assert(_ringbuffer == nullptr);
-
-	_size = buffer_size;
-	_ringbuffer = new uint8_t[_size];
-	return _ringbuffer != nullptr;
-}
-
-void Ringbuffer::deallocate()
-{
-	delete[] _ringbuffer;
-	_ringbuffer = nullptr;
-	_size = 0;
-}
-
-size_t Ringbuffer::space_available() const
-{
-	if (_start > _end) {
-		return _start - _end - 1;
-
-	} else {
-		return _start - _end - 1 + _size;
-	}
-}
-
-size_t Ringbuffer::space_used() const
-{
-	if (_start <= _end) {
-		return _end - _start;
-
-	} else {
-		// Potential wrap around.
-		return _end - _start + _size;
-	}
-}
-
-
-bool Ringbuffer::push_back(const uint8_t *buf, size_t buf_len)
-{
-	if (buf_len == 0 || buf == nullptr) {
-		// Nothing to add, we better don't try.
-		return false;
-	}
-
-	if (_start > _end) {
-		// Add after end up to start, no wrap around.
-
-		// Leave one byte free so that start don't end up the same
-		// which signals empty.
-		const size_t available = _start - _end - 1;
-
-		if (available < buf_len) {
-			return false;
-		}
-
-		memcpy(&_ringbuffer[_end], buf, buf_len);
-		_end += buf_len;
-
-	} else {
-		// Add after end, maybe wrap around.
-		const size_t available = _start - _end - 1 + _size;
-
-		if (available < buf_len) {
-			return false;
-		}
-
-		const size_t remaining_packet_len = _size - _end;
-
-		if (buf_len > remaining_packet_len) {
-			memcpy(&_ringbuffer[_end], buf, remaining_packet_len);
-			_end = 0;
-
-			memcpy(&_ringbuffer[_end], buf + remaining_packet_len, buf_len - remaining_packet_len);
-			_end += buf_len - remaining_packet_len;
-
-		} else {
-			memcpy(&_ringbuffer[_end], buf, buf_len);
-			_end += buf_len;
-		}
-	}
-
-	return true;
-}
-
-size_t Ringbuffer::pop_front(uint8_t *buf, size_t buf_max_len)
-{
-	if (buf == nullptr) {
-		// User needs to supply a valid pointer.
-		return 0;
-	}
-
-	if (_start == _end) {
-		// Empty
-		return 0;
-	}
-
-	if (_start < _end) {
-
-		// No wrap around.
-		size_t to_copy_len = math::min(_end - _start, buf_max_len);
-
-		memcpy(buf, &_ringbuffer[_start], to_copy_len);
-		_start += to_copy_len;
-
-		return to_copy_len;
-
-	} else {
-		// Potential wrap around.
-		size_t to_copy_len = _end - _start + _size;
-
-		if (to_copy_len > buf_max_len) {
-			to_copy_len = buf_max_len;
-		}
-
-		const size_t remaining_buf_len = _size - _start;
-
-		if (to_copy_len > remaining_buf_len) {
-
-			memcpy(buf, &_ringbuffer[_start], remaining_buf_len);
-			_start = 0;
-			memcpy(buf + remaining_buf_len, &_ringbuffer[_start], to_copy_len - remaining_buf_len);
-			_start += to_copy_len - remaining_buf_len;
-
-		} else {
-			memcpy(buf, &_ringbuffer[_start], to_copy_len);
-			_start += to_copy_len;
-		}
-
-		return to_copy_len;
-	}
-}

src/lib/ringbuffer/Ringbuffer.hpp

@@ -1,120 +0,0 @@
-/****************************************************************************
- *
- *   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 <stdint.h>
-#include <pthread.h>
-
-
-// FIFO ringbuffer implementation.
-//
-// The ringbuffer can store up 1 byte less than allocated as
-// start and end marker need to be one byte apart when the buffer
-// is full, otherwise it would suddenly be empty.
-//
-// The buffer is not thread-safe.
-
-class Ringbuffer
-{
-public:
-	/* @brief Constructor
-	 *
-	 * @note Does not allocate automatically.
-	 */
-	Ringbuffer() = default;
-
-	/*
-	 * @brief Destructor
-	 *
-	 * Automatically calls deallocate.
-	 */
-	~Ringbuffer();
-
-	/* @brief Allocate ringbuffer
-	 *
-	 * @param buffer_size Number of bytes to allocate on heap.
-	 *
-	 * @returns false if allocation fails.
-	 */
-	bool allocate(size_t buffer_size);
-
-	/*
-	 * @brief Deallocate ringbuffer
-	 *
-	 * @note only required to deallocate and reallocate again.
-	 */
-	void deallocate();
-
-	/*
-	 * @brief Space available to copy bytes into
-	 *
-	 * @returns number of free bytes.
-	 */
-	size_t space_available() const;
-
-	/*
-	 * @brief Space used to copy data from
-	 *
-	 * @returns number of used bytes.
-	 */
-	size_t space_used() const;
-
-	/*
-	 * @brief Copy data into ringbuffer
-	 *
-	 * @param buf Pointer to buffer to copy from.
-	 * @param buf_len Number of bytes to copy.
-	 *
-	 * @returns true if packet could be copied into buffer.
-	 */
-	bool push_back(const uint8_t *buf, size_t buf_len);
-
-	/*
-	 * @brief Get data from ringbuffer
-	 *
-	 * @param buf Pointer to buffer where data can be copied into.
-	 * @param max_buf_len Max number of bytes to copy.
-	 *
-	 * @returns 0 if buffer is empty.
-	 */
-	size_t pop_front(uint8_t *buf, size_t max_buf_len);
-
-private:
-	size_t _size {0};
-	uint8_t *_ringbuffer {nullptr};
-	size_t _start{0};
-	size_t _end{0};
-};

src/lib/ringbuffer/RingbufferTest.cpp

@@ -1,247 +0,0 @@
-/****************************************************************************
- *
- *   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 <gtest/gtest.h>
-#include <stdint.h>
-#include <string.h>
-
-
-#include "Ringbuffer.hpp"
-
-class TempData
-{
-public:
-	TempData(size_t len)
-	{
-		_size = len;
-		_buf = new uint8_t[_size];
-	}
-
-	~TempData()
-	{
-		delete[] _buf;
-		_buf = nullptr;
-	}
-
-	uint8_t *buf() const
-	{
-		return _buf;
-	}
-
-	size_t size() const
-	{
-		return _size;
-	}
-
-	void paint(unsigned offset = 0)
-	{
-		for (size_t i = 0; i < _size; ++i) {
-			_buf[i] = (uint8_t)((i + offset) % UINT8_MAX);
-		}
-	}
-
-private:
-	uint8_t *_buf {nullptr};
-	size_t _size{0};
-
-};
-
-bool operator==(const TempData &lhs, const TempData &rhs)
-{
-	if (lhs.size() != rhs.size()) {
-		return false;
-	}
-
-	return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0;
-}
-
-
-TEST(Ringbuffer, AllocateAndDeallocate)
-{
-	Ringbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-	buf.deallocate();
-
-	ASSERT_TRUE(buf.allocate(1000));
-	// The second time we forget to clean up, but we expect no leak.
-}
-
-TEST(Ringbuffer, PushATooBigMessage)
-{
-	Ringbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	TempData data{200};
-
-	// A message that doesn't fit should get rejected.
-	EXPECT_FALSE(buf.push_back(data.buf(), data.size()));
-}
-
-TEST(Ringbuffer, PushAndPopOne)
-{
-	Ringbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	TempData data{20};
-	data.paint();
-
-	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-
-	EXPECT_EQ(buf.space_used(), 20);
-	EXPECT_EQ(buf.space_available(), 79);
-
-	// Get everything
-	TempData out{20};
-	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20);
-	EXPECT_EQ(data, out);
-
-	// Nothing remaining
-	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0);
-}
-
-TEST(Ringbuffer, PushAndPopSeveral)
-{
-	Ringbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	TempData data{90};
-	data.paint();
-
-	// 9 little chunks in
-	for (unsigned i = 0; i < 9; ++i) {
-		EXPECT_TRUE(buf.push_back(data.buf() + i * 10, 10));
-	}
-
-	// 10 won't because of overhead inside the buffer
-	EXPECT_FALSE(buf.push_back(data.buf(), 10));
-
-	TempData out{90};
-	// Take it back out in 2 big steps
-	EXPECT_EQ(buf.pop_front(out.buf(), 50), 50);
-	EXPECT_EQ(buf.pop_front(out.buf() + 50, 40), 40);
-	EXPECT_EQ(data, out);
-}
-
-TEST(Ringbuffer, PushAndTryToPopMore)
-{
-	Ringbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	TempData data1{50};
-	data1.paint();
-	EXPECT_TRUE(buf.push_back(data1.buf(), data1.size()));
-
-	TempData out1{80};
-	EXPECT_EQ(buf.pop_front(out1.buf(), out1.size()), data1.size());
-}
-
-TEST(Ringbuffer, PushAndPopSeveralInterleaved)
-{
-	Ringbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	TempData data1{50};
-	data1.paint();
-	EXPECT_TRUE(buf.push_back(data1.buf(), data1.size()));
-
-	TempData data2{30};
-	data2.paint(50);
-	EXPECT_TRUE(buf.push_back(data2.buf(), data2.size()));
-
-	TempData out12{80};
-	EXPECT_EQ(buf.pop_front(out12.buf(), out12.size()), out12.size());
-
-	TempData out12_ref{80};
-	out12_ref.paint();
-	EXPECT_EQ(out12_ref, out12);
-
-	TempData data3{50};
-	data3.paint(33);
-	EXPECT_TRUE(buf.push_back(data3.buf(), data3.size()));
-
-	TempData out3{50};
-	EXPECT_EQ(buf.pop_front(out3.buf(), out3.size()), data3.size());
-	EXPECT_EQ(data3, out3);
-}
-
-TEST(Ringbuffer, PushEmpty)
-{
-	Ringbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	EXPECT_FALSE(buf.push_back(nullptr, 0));
-}
-
-TEST(Ringbuffer, PopWithoutBuffer)
-{
-	Ringbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	EXPECT_FALSE(buf.push_back(nullptr, 0));
-
-	TempData data{50};
-	data.paint();
-
-	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-
-
-	EXPECT_EQ(buf.pop_front(nullptr, 50), 0);
-}
-
-TEST(Ringbuffer, EmptyAndNoSpaceForHeader)
-{
-	// Addressing a corner case where start and end are at the end
-	// and the same.
-
-	Ringbuffer buf;
-	// Allocate 1 bytes more than the packet, 1 for the start/end logic.
-	ASSERT_TRUE(buf.allocate(21));
-
-	{
-		TempData data{20};
-		data.paint();
-		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-		TempData out{20};
-		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
-		EXPECT_EQ(data, out);
-	}
-
-	{
-		TempData data{10};
-		data.paint();
-		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-		TempData out{10};
-		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
-		EXPECT_EQ(data, out);
-	}
-}

src/lib/tecs/TECS.cpp

@@ -52,17 +52,10 @@ using namespace time_literals;
 
 static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);}
 
-void TECSAirspeedFilter::initialize(const float equivalent_airspeed, const float equivalent_airspeed_trim,
-				    const bool airspeed_sensor_available)
+void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
 {
-	if (airspeed_sensor_available && PX4_ISFINITE(equivalent_airspeed)) {
-		_airspeed_state.speed = equivalent_airspeed;
-
-	} else {
-		_airspeed_state.speed = equivalent_airspeed_trim;
-	}
-
-	_airspeed_state.speed_rate = 0.f;
+	_airspeed_state.speed = equivalent_airspeed;
+	_airspeed_state.speed_rate = 0.0f;
 }
 
 void TECSAirspeedFilter::update(const float dt, const Input &input, const Param &param,
@@ -360,7 +353,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt
 
 void TECSControl::_detectUnderspeed(const Input &input, const Param &param, const Flag &flag)
 {
-	if (!flag.detect_underspeed_enabled || !flag.airspeed_enabled) {
+	if (!flag.detect_underspeed_enabled) {
 		_ratio_undersped = 0.0f;
 		return;
 	}
@@ -419,7 +412,6 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific
 	const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON);
 	_pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment,
 				    _pitch_setpoint + pitch_increment);
-	_pitch_setpoint = constrain(_pitch_setpoint, param.pitch_min, param.pitch_max);
 
 	//Debug Output
 	_debug_output.energy_balance_rate_estimate = seb_rate.estimate;
@@ -662,8 +654,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 	TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude,
 			.alt_rate = altitude_rate};
 	_altitude_reference_model.initialize(current_state);
-	_airspeed_filter.initialize(equivalent_airspeed, _airspeed_filter_param.equivalent_airspeed_trim,
-				    _control_flag.airspeed_enabled);
+	_airspeed_filter.initialize(equivalent_airspeed);
 
 	TECSControl::Setpoint control_setpoint;
 	control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();

src/lib/tecs/TECS.hpp

@@ -88,11 +88,8 @@ public:
 	 * @brief Initialize filter
 	 *
 	 * @param[in] equivalent_airspeed is the equivalent airspeed in [m/s].
-	 * @param[in] equivalent_airspeed_trim is the equivalent airspeed trim (vehicle setting) in [m/s].
-	 * @param[in] airspeed_sensor_available boolean if the airspeed sensor is available.
 	 */
-	void initialize(float equivalent_airspeed, const float equivalent_airspeed_trim,
-			const bool airspeed_sensor_available);
+	void initialize(float equivalent_airspeed);
 
 	/**
 	 * @brief Update filter
@@ -699,9 +696,9 @@ private:
 		.max_climb_rate = 5.0f,
 		.vert_accel_limit = 0.0f,
 		.equivalent_airspeed_trim = 15.0f,
-		.tas_min = 10.0f,
-		.pitch_max = 0.5f,
-		.pitch_min = -0.5f,
+		.tas_min = 3.0f,
+		.pitch_max = 5.0f,
+		.pitch_min = -5.0f,
 		.throttle_trim = 0.0f,
 		.throttle_trim_adjusted = 0.f,
 		.throttle_max = 1.0f,

src/lib/variable_length_ringbuffer/CMakeLists.txt

@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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_library(variable_length_ringbuffer
-	VariableLengthRingbuffer.cpp
-)
-
-target_link_libraries(variable_length_ringbuffer PRIVATE ringbuffer)
-
-target_include_directories(variable_length_ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-px4_add_unit_gtest(SRC VariableLengthRingbufferTest.cpp LINKLIBS variable_length_ringbuffer)

src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp

@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- *   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 "VariableLengthRingbuffer.hpp"
-
-#include <assert.h>
-#include <string.h>
-
-
-VariableLengthRingbuffer::~VariableLengthRingbuffer()
-{
-	deallocate();
-}
-
-bool VariableLengthRingbuffer::allocate(size_t buffer_size)
-{
-	return _ringbuffer.allocate(buffer_size);
-}
-
-void VariableLengthRingbuffer::deallocate()
-{
-	_ringbuffer.deallocate();
-}
-
-bool VariableLengthRingbuffer::push_back(const uint8_t *packet, size_t packet_len)
-{
-	if (packet_len == 0 || packet == nullptr) {
-		// Nothing to add, we better don't try.
-		return false;
-	}
-
-	size_t space_required = packet_len + sizeof(Header);
-
-	if (space_required > _ringbuffer.space_available()) {
-		return false;
-	}
-
-	Header header{static_cast<uint32_t>(packet_len)};
-	bool result = _ringbuffer.push_back(reinterpret_cast<const uint8_t * >(&header), sizeof(header));
-	assert(result);
-
-	result = _ringbuffer.push_back(packet, packet_len);
-	assert(result);
-
-	// In case asserts are commented out to prevent unused warnings.
-	(void)result;
-
-	return true;
-}
-
-size_t VariableLengthRingbuffer::pop_front(uint8_t *buf, size_t buf_max_len)
-{
-	if (buf == nullptr) {
-		// User needs to supply a valid pointer.
-		return 0;
-	}
-
-	// Check next header
-	Header header;
-
-	if (_ringbuffer.pop_front(reinterpret_cast<uint8_t *>(&header), sizeof(header)) < sizeof(header)) {
-		return 0;
-	}
-
-	// We can't fit the packet into the user supplied buffer.
-	// This should never happen as the user has to supply a big // enough buffer.
-	assert(static_cast<uint32_t>(header.len) <= buf_max_len);
-
-	size_t bytes_read = _ringbuffer.pop_front(buf, header.len);
-	assert(bytes_read == header.len);
-
-	return bytes_read;
-}

src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp

@@ -1,111 +0,0 @@
-/****************************************************************************
- *
- *   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 <stdint.h>
-#include <lib/ringbuffer/Ringbuffer.hpp>
-
-
-// FIFO ringbuffer implementation for packets of variable length.
-//
-// The variable length is implemented using a 4 byte header
-// containing a the length.
-//
-// The buffer is not thread-safe.
-
-class VariableLengthRingbuffer
-{
-public:
-	/* @brief Constructor
-	 *
-	 * @note Does not allocate automatically.
-	 */
-	VariableLengthRingbuffer() = default;
-
-	/*
-	 * @brief Destructor
-	 *
-	 * Automatically calls deallocate.
-	 */
-	~VariableLengthRingbuffer();
-
-	/* @brief Allocate ringbuffer
-	 *
-	 * @note The variable length requires 4 bytes
-	 * of overhead per packet.
-	 *
-	 * @param buffer_size Number of bytes to allocate on heap.
-	 *
-	 * @returns false if allocation fails.
-	 */
-	bool allocate(size_t buffer_size);
-
-	/*
-	 * @brief Deallocate ringbuffer
-	 *
-	 * @note only required to deallocate and reallocate again.
-	 */
-	void deallocate();
-
-	/*
-	 * @brief Copy packet into ringbuffer
-	 *
-	 * @param packet Pointer to packet to copy from.
-	 * @param packet_len Length of packet.
-	 *
-	 * @returns true if packet could be copied into buffer.
-	 */
-	bool push_back(const uint8_t *packet, size_t packet_len);
-
-	/*
-	 * @brief Get packet from ringbuffer
-	 *
-	 * @note max_buf_len needs to be bigger equal to any pushed packet.
-	 *
-	 * @param buf Pointer to where next packet can be copied into.
-	 * @param max_buf_len Max size of buf
-	 *
-	 * @returns 0 if packet is bigger than max_len or buffer is empty.
-	 */
-	size_t pop_front(uint8_t *buf, size_t max_buf_len);
-
-private:
-	struct Header {
-		uint32_t len;
-	};
-
-	Ringbuffer _ringbuffer {};
-};

src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp

@@ -1,257 +0,0 @@
-/****************************************************************************
- *
- *   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 <gtest/gtest.h>
-#include <stdint.h>
-#include <string.h>
-
-
-#include "VariableLengthRingbuffer.hpp"
-
-class TempData
-{
-public:
-	TempData(size_t len)
-	{
-		_size = len;
-		_buf = new uint8_t[_size];
-	}
-
-	~TempData()
-	{
-		delete[] _buf;
-		_buf = nullptr;
-	}
-
-	uint8_t *buf() const
-	{
-		return _buf;
-	}
-
-	size_t size() const
-	{
-		return _size;
-	}
-
-	void paint(unsigned offset = 0)
-	{
-		for (size_t i = 0; i < _size; ++i) {
-			_buf[i] = (uint8_t)((i + offset) % UINT8_MAX);
-		}
-	}
-
-private:
-	uint8_t *_buf {nullptr};
-	size_t _size{0};
-
-};
-
-bool operator==(const TempData &lhs, const TempData &rhs)
-{
-	if (lhs.size() != rhs.size()) {
-		return false;
-	}
-
-	return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0;
-}
-
-
-TEST(VariableLengthRingbuffer, AllocateAndDeallocate)
-{
-	VariableLengthRingbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-	buf.deallocate();
-
-	ASSERT_TRUE(buf.allocate(1000));
-	// The second time we forget to clean up, but we expect no leak.
-}
-
-TEST(VariableLengthRingbuffer, PushATooBigMessage)
-{
-	VariableLengthRingbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	TempData data{200};
-
-	// A message that doesn't fit should get rejected.
-	EXPECT_FALSE(buf.push_back(data.buf(), data.size()));
-}
-
-TEST(VariableLengthRingbuffer, PushAndPopOne)
-{
-	VariableLengthRingbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	TempData data{20};
-	data.paint();
-
-	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-
-
-	// Out buffer is the same size
-	TempData out{20};
-	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20);
-	EXPECT_EQ(data, out);
-
-	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-	// Out buffer is supposedly bigger
-	TempData out2{20};
-	EXPECT_EQ(buf.pop_front(out2.buf(), 21), 20);
-	EXPECT_EQ(data, out2);
-
-	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-
-	// Disabled because it doesn't work reliably.
-	// For some reason the abort works when tests are filtered using TESTFILTER
-	// but not when all tests are run.
-	//
-	// Out buffer is too small
-	// Asserts are disabled in release build
-	//TempData out3{19};
-	//EXPECT_DEATH(buf.pop_front(out3.buf(), out3.size()), ".*");
-}
-
-TEST(VariableLengthRingbuffer, PushAndPopSeveral)
-{
-	VariableLengthRingbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	TempData data{20};
-	data.paint();
-
-	// 4 should fit
-	for (unsigned i = 0; i < 4; ++i) {
-		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-	}
-
-	// 5 won't because of overhead
-	EXPECT_FALSE(buf.push_back(data.buf(), data.size()));
-
-	// Take 4 back out
-	for (unsigned i = 0; i < 4; ++i) {
-		TempData out{20};
-		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), data.size());
-		EXPECT_EQ(data, out);
-	}
-
-	TempData out{20};
-	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0);
-}
-
-TEST(VariableLengthRingbuffer, PushAndPopSeveralVariableSize)
-{
-	VariableLengthRingbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	TempData data1{50};
-	data1.paint();
-	EXPECT_TRUE(buf.push_back(data1.buf(), data1.size()));
-
-	TempData data2{30};
-	data2.paint(42);
-	EXPECT_TRUE(buf.push_back(data2.buf(), data2.size()));
-
-	// Supposedly more space
-	TempData out1{50};
-	EXPECT_EQ(buf.pop_front(out1.buf(), 100), data1.size());
-	EXPECT_EQ(data1, out1);
-
-	TempData data3{50};
-	data3.paint(33);
-	EXPECT_TRUE(buf.push_back(data3.buf(), data3.size()));
-
-	// Supposedly more space
-	TempData out2{30};
-	EXPECT_EQ(buf.pop_front(out2.buf(), 100), data2.size());
-	EXPECT_EQ(data2, out2);
-
-	// Supposedly more space
-	TempData out3{50};
-	EXPECT_EQ(buf.pop_front(out3.buf(), 100), data3.size());
-	EXPECT_EQ(data3, out3);
-
-	TempData out4{100};
-	EXPECT_EQ(buf.pop_front(out4.buf(), out4.size()), 0);
-}
-
-TEST(VariableLengthRingbuffer, PushEmpty)
-{
-	VariableLengthRingbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	EXPECT_FALSE(buf.push_back(nullptr, 0));
-}
-
-TEST(VariableLengthRingbuffer, PopWithoutBuffer)
-{
-	VariableLengthRingbuffer buf;
-	ASSERT_TRUE(buf.allocate(100));
-
-	EXPECT_FALSE(buf.push_back(nullptr, 0));
-
-	TempData data{50};
-	data.paint();
-
-	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-
-
-	EXPECT_EQ(buf.pop_front(nullptr, 50), 0);
-}
-
-TEST(VariableLengthRingbuffer, EmptyAndNoSpaceForHeader)
-{
-	// Addressing a corner case where start and end are at the end
-	// and the same.
-
-	VariableLengthRingbuffer buf;
-	// Allocate 4+1 bytes more than the packet, 4 for the header, 1 for the start/end logic.
-	ASSERT_TRUE(buf.allocate(25));
-
-	{
-		TempData data{20};
-		data.paint();
-		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-		TempData out{20};
-		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
-		EXPECT_EQ(data, out);
-	}
-
-	{
-		TempData data{10};
-		data.paint();
-		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
-		TempData out{10};
-		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
-		EXPECT_EQ(data, out);
-	}
-}

src/lib/version/version.h

@@ -88,16 +88,6 @@ static inline int px4_board_hw_revision(void)
 	return board_get_hw_revision();
 }
 
-#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
-/**
- * get the base board type
- */
-static inline const char *px4_board_base_type(void)
-{
-	return board_get_hw_base_type_name();
-}
-#endif
-
 /**
  * get the build URI (used for crash logging)
  */

src/modules/commander/Commander.cpp

@@ -1367,10 +1367,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		break;
 
-	case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR:
-		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
-		break;
-
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
@@ -1407,6 +1403,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
 	case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
 	case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
+	case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR:
 	case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
 	case vehicle_command_s::VEHICLE_CMD_DO_WINCH:
 	case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -229,9 +229,6 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co
 
 	} else if (_saturation_flags.roll_neg) {
 		status.unallocated_torque[0] = -1.f;
-
-	} else {
-		status.unallocated_torque[0] = 0.f;
 	}
 
 	if (_saturation_flags.pitch_pos) {
@@ -239,9 +236,6 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co
 
 	} else if (_saturation_flags.pitch_neg) {
 		status.unallocated_torque[1] = -1.f;
-
-	} else {
-		status.unallocated_torque[1] = 0.f;
 	}
 
 	if (_saturation_flags.yaw_pos) {
@@ -249,9 +243,6 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co
 
 	} else if (_saturation_flags.yaw_neg) {
 		status.unallocated_torque[2] = -1.f;
-
-	} else {
-		status.unallocated_torque[2] = 0.f;
 	}
 
 	if (_saturation_flags.thrust_pos) {
@@ -259,8 +250,5 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co
 
 	} else if (_saturation_flags.thrust_neg) {
 		status.unallocated_thrust[2] = -1.f;
-
-	} else {
-		status.unallocated_thrust[2] = 0.f;
 	}
 }

src/modules/ekf2/EKF/range_finder_consistency_check.cpp

@@ -37,12 +37,8 @@
 
 #include "range_finder_consistency_check.hpp"
 
-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
+void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us)
 {
-	if (horizontal_motion) {
-		_time_last_horizontal_motion = time_us;
-	}
-
 	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
 
 	if ((_time_last_update_us == 0)
@@ -72,20 +68,12 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
 
 void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
 {
-	if (fabsf(vz) < _min_vz_for_valid_consistency) {
-		// We can only check consistency if there is vertical motion
-		return;
-	}
-
 	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
-		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
-			_is_kinematically_consistent = false;
-			_time_last_inconsistent_us = time_us;
-		}
+		_is_kinematically_consistent = false;
+		_time_last_inconsistent_us = time_us;
 
 	} else {
-		if (_test_ratio < 1.f
-		   && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
+		if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
 			_is_kinematically_consistent = true;
 		}
 	}

src/modules/ekf2/EKF/range_finder_consistency_check.hpp

@@ -47,7 +47,7 @@ public:
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;
 
-	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);
+	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us);
 
 	void setGate(float gate) { _gate = gate; }
 
@@ -71,7 +71,6 @@ private:
 
 	bool _is_kinematically_consistent{true};
 	uint64_t _time_last_inconsistent_us{};
-	uint64_t _time_last_horizontal_motion{};
 
 	static constexpr float _signed_test_ratio_tau = 2.f;
 

src/modules/ekf2/EKF/range_height_control.cpp

@@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion()
 			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 
-			if (_control_status.flags.in_air) {
-				const bool horizontal_motion = _control_status.flags.fixed_wing
-								|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P(4, 4) + P(5, 5), 0.1f));
+			// Run the kinematic consistency check when not moving horizontally
+			if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
+			    && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
 
 				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
 				const float var = sq(_params.range_noise) + dist_dependant_var;
 
 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), horizontal_motion, _time_delayed_us);
+				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us);
 			}
 
 		} else {

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -199,6 +199,7 @@ void Ekf::resetHaglRng()
 	_terrain_var = getRngVar();
 	_terrain_vpos_reset_counter++;
 	_time_last_hagl_fuse = _time_delayed_us;
+	_time_last_healthy_rng_data = 0;
 }
 
 void Ekf::stopHaglRngFusion()

src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv

@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 90000,1,-0.0096,-0.01,-0.02,-0.0004,0.0026,-0.026,-1.4e-05,0.00011,-0.0023,0,0,0,0,0,0,0.19,-4.7e-10,0.4,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 190000,1,-0.0096,-0.011,-0.02,0.0002,0.0039,-0.041,6e-06,0.00043,-0.0044,-3e-13,-2.3e-14,5.8e-15,0,0,-2e-11,0.19,-4.7e-10,0.4,0,0,0,0,0,5.8e-07,0.0027,0.0027,0.0008,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 290000,1,-0.0097,-0.011,-0.02,0.0012,0.0063,-0.053,5e-05,0.00036,-0.007,9.1e-12,9.1e-13,-1.7e-13,0,0,-4.8e-09,0.19,-4.7e-10,0.4,0,0,0,0,0,6.8e-07,0.0029,0.0029,0.00067,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
@@ -132,7 +132,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 12990000,0.7,0.0012,-0.014,0.71,-0.0078,0.0067,-0.03,-0.0011,0.0013,-3.7e+02,-1.2e-05,-5.9e-05,-2.3e-06,6.8e-06,9.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.062,0.062,0.025,0.049,0.049,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0
 13090000,0.7,0.0012,-0.014,0.71,-0.0084,0.0069,-0.03,-0.0019,0.002,-3.7e+02,-1.2e-05,-5.8e-05,-2.8e-06,6.4e-06,9.8e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00021,0.00021,9.3e-05,0.071,0.071,0.025,0.057,0.057,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0
 13190000,0.7,0.00095,-0.014,0.71,0.00044,0.0063,-0.027,0.0043,0.0013,-3.7e+02,-1.1e-05,-5.9e-05,-2.8e-06,5.1e-06,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.058,0.058,0.025,0.049,0.049,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0
-13290000,0.7,0.00096,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0
+13290000,0.7,0.00095,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0
 13390000,0.7,0.00081,-0.014,0.71,0.001,0.0062,-0.02,0.0032,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-2e-06,2.6e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.055,0.055,0.026,0.048,0.048,0.05,3.5e-09,3.5e-09,1.9e-09,3.5e-06,3.5e-06,8.8e-07,0,0,0,0,0,0,0,0
 13490000,0.7,0.00084,-0.014,0.71,0.0006,0.0062,-0.019,0.0033,0.0018,-3.7e+02,-1.1e-05,-5.9e-05,-1.7e-06,1.8e-06,1.5e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.063,0.063,0.028,0.056,0.056,0.05,3.5e-09,3.5e-09,1.8e-09,3.5e-06,3.5e-06,8.7e-07,0,0,0,0,0,0,0,0
 13590000,0.7,0.00078,-0.014,0.71,0.0008,0.0064,-0.021,0.0023,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-1.9e-06,1.7e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.052,0.052,0.028,0.048,0.048,0.05,3.3e-09,3.4e-09,1.8e-09,3.5e-06,3.5e-06,8.4e-07,0,0,0,0,0,0,0,0

src/modules/ekf2/test/change_indication/iris_gps.csv

@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 90000,0.98,-0.0095,-0.012,0.18,-5.5e-05,-0.0032,-0.024,-3.6e-06,-0.00014,-0.0021,0,0,0,0,0,0,0.2,-3.3e-09,0.43,0,0,0,0,0,8.9e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 190000,0.98,-0.0092,-0.013,0.21,-0.0013,-0.0036,-0.037,-4.4e-05,-0.00046,-0.017,5.2e-12,-4.3e-12,-1.5e-13,0,0,-6.8e-10,0.2,0.011,0.43,0,0,0,0,0,2.8e-06,0.0027,0.0027,0.00081,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 290000,0.98,-0.0092,-0.013,0.21,-0.0016,-0.0053,-0.046,-0.00015,-0.00032,-0.018,4.4e-11,-5.4e-11,-2.6e-12,0,0,-2.9e-08,0.2,0.011,0.43,0,0,0,0,0,6.6e-06,0.0029,0.0029,0.00068,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
@@ -347,5 +347,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 34490000,0.98,-0.0069,-0.013,0.18,-0.016,-0.0096,-0.09,0.071,-0.011,-0.11,-1.4e-05,-5.6e-05,2.3e-06,0.00017,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.3e-05,3.3e-05,4.2e-05,0.053,0.053,0.0059,0.051,0.051,0.032,2.6e-11,2.6e-11,8.1e-11,2.3e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0
 34590000,0.98,-0.0069,-0.013,0.18,-0.014,-0.0053,0.71,0.073,-0.0089,-0.081,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3e-05,4.2e-05,0.044,0.044,0.0059,0.045,0.045,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0
 34690000,0.98,-0.0068,-0.012,0.18,-0.017,-0.0032,1.7,0.071,-0.0093,0.037,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3.1e-05,4.2e-05,0.047,0.047,0.006,0.052,0.052,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0
-34790000,0.98,-0.0068,-0.012,0.18,-0.019,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0
+34790000,0.98,-0.0068,-0.012,0.18,-0.018,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0
 34890000,0.98,-0.0068,-0.012,0.18,-0.022,0.0039,3.6,0.07,-0.0065,0.5,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.043,0.043,0.0061,0.052,0.052,0.032,2.6e-11,2.6e-11,7.8e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -127,6 +127,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
 				// Stop using distance to ground
 				_terrain_hold = false;
+				_terrain_follow = false;
 
 				// Adjust the setpoint to maintain the same height error to reduce control transients
 				if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
@@ -143,6 +144,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
 				// Start using distance to ground
 				_terrain_hold = true;
+				_terrain_follow = true;
 
 				// Adjust the setpoint to maintain the same height error to reduce control transients
 				if (PX4_ISFINITE(_position_setpoint(2))) {
@@ -153,7 +155,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 
 	}
 
-	if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) {
+	if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) {
 		// terrain following
 		_terrainFollowing(apply_brake, stopped);
 		// respect maximum altitude