From b50d405f91de724840d2a4e140bcae2047e38c93 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 19 Jul 2022 08:30:56 +1000
Subject: [PATCH] AP_BoardConfig: added board detection for FMUv6

support Holybro-6X and CUAV-6X for now, using IMU signatures
---
 libraries/AP_BoardConfig/AP_BoardConfig.h  |  3 +++
 libraries/AP_BoardConfig/board_drivers.cpp | 24 ++++++++++++++++++++--
 2 files changed, 25 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h
index 4569266807..46797e94b4 100644
--- a/libraries/AP_BoardConfig/AP_BoardConfig.h
+++ b/libraries/AP_BoardConfig/AP_BoardConfig.h
@@ -96,6 +96,8 @@ public:
         VRX_BOARD_CORE10   = 36,
         VRX_BOARD_BRAIN54  = 38,
         PX4_BOARD_FMUV6    = 39,
+        FMUV6_BOARD_HOLYBRO_6X = 40,
+        FMUV6_BOARD_CUAV_6X = 41,
         PX4_BOARD_OLDDRIVERS = 100,
     };
 
@@ -249,6 +251,7 @@ private:
     bool spi_check_register_inv2(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag = 0x80);
     void validate_board_type(void);
     void board_autodetect(void);
+    void detect_fmuv6_variant(void);
     bool check_ms5611(const char* devname);
 
 #endif // AP_FEATURE_BOARD_DETECT
diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp
index 823da14e08..e61e5f4d44 100644
--- a/libraries/AP_BoardConfig/board_drivers.cpp
+++ b/libraries/AP_BoardConfig/board_drivers.cpp
@@ -117,6 +117,8 @@ void AP_BoardConfig::board_setup_drivers(void)
     case PX4_BOARD_PIXHAWK_PRO:
     case PX4_BOARD_PCNC1:
     case PX4_BOARD_MINDPXV2:
+    case FMUV6_BOARD_HOLYBRO_6X:
+    case FMUV6_BOARD_CUAV_6X:
         break;
     default:
         config_error("Unknown board type");
@@ -370,8 +372,7 @@ void AP_BoardConfig::board_autodetect(void)
     state.board_type.set_and_notify(PX4_BOARD_FMUV5);
     DEV_PRINTF("Detected FMUv5\n");
 #elif defined(HAL_CHIBIOS_ARCH_FMUV6)
-    state.board_type.set_and_notify(PX4_BOARD_FMUV5);
-    DEV_PRINTF("Detected FMUv6\n");
+    detect_fmuv6_variant();
 #elif defined(HAL_CHIBIOS_ARCH_BRAINV51)
     state.board_type.set_and_notify(VRX_BOARD_BRAIN51);
     DEV_PRINTF("Detected VR Brain 5.1\n");
@@ -486,3 +487,22 @@ void AP_BoardConfig::board_setup()
 #endif
 }
 
+
+#ifdef HAL_CHIBIOS_ARCH_FMUV6
+/*
+  detect which FMUV6 variant we are running on
+ */
+void AP_BoardConfig::detect_fmuv6_variant()
+{
+    if ((spi_check_register_inv2("icm20649", INV2REG_WHOAMI, INV2_WHOAMI_ICM20649) &&
+         spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688))) {
+        state.board_type.set_and_notify(FMUV6_BOARD_HOLYBRO_6X);
+        DEV_PRINTF("Detected Holybro 6X\n");
+    } else if ((spi_check_register_inv2("icm20649_2", INV2REG_WHOAMI, INV2_WHOAMI_ICM20649) &&
+                spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688))) {
+        state.board_type.set_and_notify(FMUV6_BOARD_CUAV_6X);
+        DEV_PRINTF("Detected CUAV 6X\n");
+    }
+
+}
+#endif