From 35eb3621f4121305682d5f77c1cf841d1dcbbe3a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 24 Oct 2023 14:52:39 +1100
Subject: [PATCH] AP_BoardConfig: detect Holybro 6X Rev6

---
 libraries/AP_BoardConfig/AP_BoardConfig.h  |  1 +
 libraries/AP_BoardConfig/board_drivers.cpp | 19 ++++++++++---------
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h
index d0a89c490df4c8..f1321c93ecf78c 100644
--- a/libraries/AP_BoardConfig/AP_BoardConfig.h
+++ b/libraries/AP_BoardConfig/AP_BoardConfig.h
@@ -67,6 +67,7 @@ class AP_BoardConfig {
         PX4_BOARD_FMUV6    = 39,
         FMUV6_BOARD_HOLYBRO_6X = 40,
         FMUV6_BOARD_CUAV_6X = 41,
+        FMUV6_BOARD_HOLYBRO_6X_REV6 = 42,
         PX4_BOARD_OLDDRIVERS = 100,
     };
 
diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp
index ed365120a08cf3..07b7fb383752ae 100644
--- a/libraries/AP_BoardConfig/board_drivers.cpp
+++ b/libraries/AP_BoardConfig/board_drivers.cpp
@@ -106,6 +106,7 @@ void AP_BoardConfig::board_setup_drivers(void)
     case PX4_BOARD_PCNC1:
     case PX4_BOARD_MINDPXV2:
     case FMUV6_BOARD_HOLYBRO_6X:
+    case FMUV6_BOARD_HOLYBRO_6X_REV6:
     case FMUV6_BOARD_CUAV_6X:
         break;
     default:
@@ -129,17 +130,15 @@ bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uin
         return false;
     }
     dev->set_read_flag(read_flag);
-    dev->get_semaphore()->take_blocking();
+    WITH_SEMAPHORE(dev->get_semaphore());
     dev->set_speed(AP_HAL::Device::SPEED_LOW);
     uint8_t v;
     if (!dev->read_registers(regnum, &v, 1)) {
 #if SPI_PROBE_DEBUG
         hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
 #endif
-        dev->get_semaphore()->give();
         return false;
     }
-    dev->get_semaphore()->give();
 #if SPI_PROBE_DEBUG
     hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
 #endif
@@ -161,7 +160,7 @@ bool AP_BoardConfig::spi_check_register_inv2(const char *devname, uint8_t regnum
         return false;
     }
     dev->set_read_flag(read_flag);
-    dev->get_semaphore()->take_blocking();
+    WITH_SEMAPHORE(dev->get_semaphore());
     dev->set_speed(AP_HAL::Device::SPEED_LOW);
     uint8_t v;
     // select bank 0 for who am i
@@ -170,10 +169,8 @@ bool AP_BoardConfig::spi_check_register_inv2(const char *devname, uint8_t regnum
 #if SPI_PROBE_DEBUG
         hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
 #endif
-        dev->get_semaphore()->give();
         return false;
     }
-    dev->get_semaphore()->give();
 #if SPI_PROBE_DEBUG
     hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
 #endif
@@ -257,12 +254,13 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
 #define INV2_WHOAMI_ICM20649 0xE1
 
 #define INV3REG_WHOAMI        0x75
-#define INV3REG_456_WHOAMI        0x72
+#define INV3REG_456_WHOAMI    0x72
 
 #define INV3_WHOAMI_ICM42688  0x47
 #define INV3_WHOAMI_ICM42670  0x67
-
 #define INV3_WHOAMI_ICM45686  0xE9
+#define INV3_WHOAMI_IIM42652  0x6f
+
 /*
   validation of the board type
  */
@@ -503,7 +501,10 @@ void AP_BoardConfig::detect_fmuv6_variant()
         state.board_type.set_and_notify(FMUV6_BOARD_CUAV_6X);
         DEV_PRINTF("Detected CUAV 6X\n");
         AP_Param::load_defaults_file("@ROMFS/param/CUAV_V6X_defaults.parm", false);
+    } else if (spi_check_register("iim42652", INV3REG_WHOAMI, INV3_WHOAMI_IIM42652) &&
+               spi_check_register("icm45686", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686)) {
+        state.board_type.set_and_notify(FMUV6_BOARD_HOLYBRO_6X_REV6);
+        DEV_PRINTF("Detected Holybro 6X_Rev6\n");
     }
-
 }
 #endif