From b09329f287b98c001c9bc846a6f87571bd035873 Mon Sep 17 00:00:00 2001
From: matthewoots <64059887+matthewoots@users.noreply.github.com>
Date: Sat, 10 Sep 2022 01:02:38 +0800
Subject: [PATCH] boards/matek/h743: add fix to resolve no startup of imu for
 matek_h743 target (#20184)

* Add fix for matek_h743 target that resolves no startup of imu for h743-wing v3 and h743-wlite boards
* Added imu driver according to spi bus (https://www.mateksys.com/?portfolio=h743-wing-v2#tab-id-7)
---
 boards/matek/h743/default.px4board      |  1 +
 boards/matek/h743/init/rc.board_sensors | 14 +++++++++-----
 boards/matek/h743/src/spi.cpp           |  1 +
 3 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/boards/matek/h743/default.px4board b/boards/matek/h743/default.px4board
index bcff78646b0..87395dd23d1 100644
--- a/boards/matek/h743/default.px4board
+++ b/boards/matek/h743/default.px4board
@@ -15,6 +15,7 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_COMMON_LIGHT=n
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
diff --git a/boards/matek/h743/init/rc.board_sensors b/boards/matek/h743/init/rc.board_sensors
index c37c3b3b586..7e970b2cde2 100644
--- a/boards/matek/h743/init/rc.board_sensors
+++ b/boards/matek/h743/init/rc.board_sensors
@@ -6,14 +6,18 @@
 board_adc start
 
 # Internal SPI bus ICM-42605
-if ! icm42605 -R 14 -s start
+if ! mpu6000 -R 12 -b 1 -s start
 then
-	# internal SPI bus ICM-20602
-	icm20602 -R 12 -s start
+	# Internal SPI bus ICM-42688P
+	icm42688p -R 12 -b 1 -s start
 fi
 
-# Internal SPI bus MPU-6000
-mpu6000 -R 12 -s start
+# internal SPI bus ICM-20602
+if ! icm20602 -R 12 -b 4 -s start
+then
+	# Internal SPI bus ICM-42605
+	icm42605 -R 14 -b 4 -s start
+fi
 
 # Internal baro
 dps310 -I start -a 118
diff --git a/boards/matek/h743/src/spi.cpp b/boards/matek/h743/src/spi.cpp
index db673bedf0d..a83701fef5b 100644
--- a/boards/matek/h743/src/spi.cpp
+++ b/boards/matek/h743/src/spi.cpp
@@ -39,6 +39,7 @@
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
 		initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}),
+		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}),