diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board
index 86f3e739ce56..4348d736c750 100644
--- a/boards/px4/fmu-v5/default.px4board
+++ b/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
diff --git a/boards/px4/fmu-v5/init/rc.board_defaults b/boards/px4/fmu-v5/init/rc.board_defaults
index 64d3288df08c..93bb336f8c5b 100644
--- a/boards/px4/fmu-v5/init/rc.board_defaults
+++ b/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
diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors
index 712e3b6d7e1d..e024e3225b2a 100644
--- a/boards/px4/fmu-v5/init/rc.board_sensors
+++ b/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
diff --git a/boards/px4/fmu-v5/src/board_config.h b/boards/px4/fmu-v5/src/board_config.h
index f1b2a0edc97a..5f20b635708e 100644
--- a/boards/px4/fmu-v5/src/board_config.h
+++ b/boards/px4/fmu-v5/src/board_config.h
@@ -186,6 +186,15 @@
 #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
+#define V500   HW_VER_REV(0x0,0x0) // FMUV5,                    Rev 0
+#define V515   HW_VER_REV(0x1,0x5) // CUAV V5,                  Rev 5
+#define V540   HW_VER_REV(0x4,0x0) // HolyBro mini no can 2,3,  Rev 0
+#define V550   HW_VER_REV(0x5,0x0) // CUAV V5+,                 Rev 0
+#define V552   HW_VER_REV(0x5,0x2) // CUAV V5+ ICM42688P,       Rev 2
+#define V560   HW_VER_REV(0x6,0x0) // CUAV V5nano with can 2,   Rev 0
+#define V562   HW_VER_REV(0x6,0x2) // CUAV V5nano ICM42688P,    Rev 2
+
 /* CAN Silence
  *
  * Silent mode control \ ESC Mux select
diff --git a/boards/px4/fmu-v5/src/manifest.c b/boards/px4/fmu-v5/src/manifest.c
index 7b1f05a3b98d..3e131f9adaaf 100644
--- a/boards/px4/fmu-v5/src/manifest.c
+++ b/boards/px4/fmu-v5/src/manifest.c
@@ -157,11 +157,13 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
 
 
 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
-	{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
+	{V500, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},
+	{V515, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},  // Alias for CUAV V5 R:5 V:1
+	{V540, hw_mft_list_v0540,        arraySize(hw_mft_list_v0540)},  // HolyBro mini no can 2,3
+	{V550, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},  // Alias for CUAV V5+ R:0 V:5
+	{V552, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},  // Alias for CUAV V5+ R:2 V:5 ICM42688P
+	{V560, hw_mft_list_v0600,        arraySize(hw_mft_list_v0600)},  // CUAV V5nano R:0 V:6 with can 2
+	{V562, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},  // Alias for CUAV V5nano R:2 V:6 ICM42688P
 };
 
 /************************************************************************************
diff --git a/boards/px4/fmu-v5/src/spi.cpp b/boards/px4/fmu-v5/src/spi.cpp
index 15d9751930b3..8a3f8c9b2999 100644
--- a/boards/px4/fmu-v5/src/spi.cpp
+++ b/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(V500, {
+		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(V552, {
+		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(V562, {
+		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);
diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp
index 6a571c9b28d1..acce069ea37b 100644
--- a/platforms/common/spi.cpp
+++ b/platforms/common/spi.cpp
@@ -47,7 +47,7 @@ void px4_set_spi_buses_from_hw_version()
 #if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
 	int hw_version_revision = board_get_hw_version();
 #else
-	int hw_version_revision = board_get_hw_revision();
+	int hw_version_revision = (board_get_hw_version() << 8) | board_get_hw_revision();
 #endif