fmu-v5: Add support for ICM-42688-P

boards/px4/fmu-v5/default.px4board

@@ -21,6 +21,7 @@ CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y

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

@@ -9,9 +9,9 @@ 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 V550 V560
+if ver hwtypecmp V550 V552 V560 V562
 then
-	# CUAV V5+ (V550) and V5nano (V560) have 3 IMUs
+	# CUAV V5+ (V550/V552) and V5nano (V560/V562) have 3 IMUs
 	# Multi-EKF (IMUs only)
 	param set-default EKF2_MULTI_IMU 3
 	param set-default SENS_IMU_MODE 0

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

@@ -5,8 +5,14 @@
 
 board_adc start
 
-# Internal SPI bus ICM-20602
-icm20602 -s -R 2 -q start
+if ver hwtypecmp V552 V562
+then
+	# Internal SPI bus ICM-42688-P
+	icm42688p -s -R 2 -q start
+else
+	# Internal SPI bus ICM-20602
+	icm20602 -s -R 2 -q start
+fi
 
 # Internal SPI bus ICM-20689
 icm20689 -s -R 2 start

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

@@ -191,6 +191,8 @@
 #define HW_INFO_INIT         {'V','5','x', 'x',0}
 #define HW_INFO_INIT_VER     2
 #define HW_INFO_INIT_REV     3
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3
+
 /* CAN Silence
  *
  * Silent mode control \ ESC Mux select

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

@@ -160,6 +160,8 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{0x0000, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},
 	{0x0105, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},  // Alias for CUAV V5 R:5 V:1
 	{0x0500, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},  // Alias for CUAV V5+ R:0 V:5
+	{0x0502, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},  // Alias for CUAV V5+ R:2 V:5 ICM42688P
+	{0x0602, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},  // Alias for CUAV V5nano R:2 V:6 ICM42688P
 	{0x0400, hw_mft_list_v0540,        arraySize(hw_mft_list_v0540)},  // HolyBro mini no can 2,3
 	{0x0600, hw_mft_list_v0600,        arraySize(hw_mft_list_v0600)},  // CUAV V5nano R:0 V:6 with can 2
 };

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

@@ -35,29 +35,81 @@
 #include <drivers/drv_sensor.h>
 #include <nuttx/spi/spi.h>
 
-constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
-	initSPIBus(SPI::Bus::SPI1, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortF, GPIO::Pin3}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}),
-		initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}),
-		initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}),
-	}, {GPIO::PortE, GPIO::Pin3}),
-	initSPIBus(SPI::Bus::SPI2, {
-		initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5})
+constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
+	initSPIHWVersion(HW_VER_REV(0, 0), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortF, GPIO::Pin3}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}),
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}),
+		}, {GPIO::PortE, GPIO::Pin3}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5})
+		}),
+		initSPIBus(SPI::Bus::SPI4, {
+			initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}),
+		}),
+		initSPIBusExternal(SPI::Bus::SPI5, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8})
+		}),
 	}),
-	initSPIBus(SPI::Bus::SPI4, {
-		initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}),
-	}),
-	initSPIBusExternal(SPI::Bus::SPI5, {
-		initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
-		initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}),
-		initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11})
+
+	initSPIHWVersion(HW_VER_REV(5, 2), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortF, GPIO::Pin11}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}),
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}),
+		}, {GPIO::PortE, GPIO::Pin3}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5})
+		}),
+		initSPIBus(SPI::Bus::SPI4, {
+			initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}),
+		}),
+		initSPIBusExternal(SPI::Bus::SPI5, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8})
+		}),
 	}),
-	initSPIBusExternal(SPI::Bus::SPI6, {
-		initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}),
-		initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}),
-		initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8})
+
+	initSPIHWVersion(HW_VER_REV(6, 2), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortF, GPIO::Pin11}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}),
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}),
+		}, {GPIO::PortE, GPIO::Pin3}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5})
+		}),
+		initSPIBus(SPI::Bus::SPI4, {
+			initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}),
+		}),
+		initSPIBusExternal(SPI::Bus::SPI5, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8})
+		}),
 	}),
 };
 
-static constexpr bool unused = validateSPIConfig(px4_spi_buses);
+static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);