From eeb9c5256ad4991d788e5388499f2880c67d1404 Mon Sep 17 00:00:00 2001
From: Andrei Korigodskii <akorigod@gmail.com>
Date: Fri, 13 Oct 2023 04:23:29 +0300
Subject: [PATCH] boards: add support for Matek H743 Slim V3

IMUs were replaced in V3 with 2x ICM42688P. This configuration should
work with all revisions of Matek F743 Slim board, including V1, V1.5
and interim variant of V3 (ICM42688P + ICM42605).

Signed-off-by: Andrei Korigodskii <akorigod@gmail.com>
---
 boards/matek/h743-slim/default.px4board      |  1 +
 boards/matek/h743-slim/init/rc.board_sensors | 22 ++++++++++++++------
 boards/matek/h743-slim/src/spi.cpp           | 12 ++++++++++-
 3 files changed, 28 insertions(+), 7 deletions(-)

diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board
index fed4e6c1a7..ce5f222229 100644
--- a/boards/matek/h743-slim/default.px4board
+++ b/boards/matek/h743-slim/default.px4board
@@ -12,6 +12,7 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
diff --git a/boards/matek/h743-slim/init/rc.board_sensors b/boards/matek/h743-slim/init/rc.board_sensors
index c37c3b3b58..e6761906ee 100644
--- a/boards/matek/h743-slim/init/rc.board_sensors
+++ b/boards/matek/h743-slim/init/rc.board_sensors
@@ -5,15 +5,25 @@
 
 board_adc start
 
-# Internal SPI bus ICM-42605
-if ! icm42605 -R 14 -s start
+# Different board versions have different IMUs, so we try all known options
+
+# Internal SPI bus ICM-42688P (SPI1) on V3 board, PITCH180 orientation
+if ! icm42688p -s -b 1 -R 12 start
 then
-	# internal SPI bus ICM-20602
-	icm20602 -R 12 -s start
+	# Internal SPI bus MPU-6000 on V1.0 and V1.5 boards
+	mpu6000 -s -b 1 -R 12 start
 fi
 
-# Internal SPI bus MPU-6000
-mpu6000 -R 12 -s start
+# Internal SPI bus ICM-42688P (SPI4) on V3 board, PITCH180_YAW90 orientation
+if ! icm42688p -s -b 4 -R 26 start
+then
+	# Internal SPI bus ICM-42605 on V1.5 board, ROTATION_ROLL_180_YAW_270 orientation
+	if ! icm42605 -s -b 4 -R 14 start
+	then
+		# Internal SPI bus ICM-20602 on V1.0 board, PITCH180 orientation
+		icm20602 -s -b 4 -R 12 start
+	fi
+fi
 
 # Internal baro
 dps310 -I start -a 118
diff --git a/boards/matek/h743-slim/src/spi.cpp b/boards/matek/h743-slim/src/spi.cpp
index db673bedf0..84f887aa69 100644
--- a/boards/matek/h743-slim/src/spi.cpp
+++ b/boards/matek/h743-slim/src/spi.cpp
@@ -38,7 +38,11 @@
 
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
+		// Matek H743 Slim V1.0 and V1.5
 		initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}),
+
+		// Matek H743 Slim V3
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}),
 	}),
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}),
@@ -48,8 +52,14 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 		initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin2}),
 	}),
 	initSPIBus(SPI::Bus::SPI4, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, SPI::CS{GPIO::PortC, GPIO::Pin13}),
+		// Matek H743 Slim V1.0
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortE, GPIO::Pin11}),
+
+		// Matek H743 Slim V1.5
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, SPI::CS{GPIO::PortC, GPIO::Pin13}),
+
+		// Matek H743 Slim V3
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}),
 	}),
 };