AP_BoardConfig: detect Holybro 6X Rev6

ArduPilot · Oct 24, 2023 · 0bf9f9a · 0bf9f9a
1 parent 203f51d
commit 0bf9f9a
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 12 deletions.
diff --git 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
@@ -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
@@ -195,7 +192,7 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
     if (!dev_sem) {
         return false;
     }
-    dev_sem->take_blocking();
+    WITH_SEMAPHORE(dev_sem);
 
     static const uint8_t CMD_MS56XX_RESET = 0x1E;
     static const uint8_t CMD_MS56XX_PROM = 0xA0;
@@ -209,7 +206,6 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
         const uint8_t reg = CMD_MS56XX_PROM + (i << 1);
         uint8_t val[2];
         if (!dev->transfer(&reg, 1, val, sizeof(val))) {
-            dev_sem->give();
 #if SPI_PROBE_DEBUG
             hal.console->printf("%s: transfer fail\n", devname);
 #endif
@@ -221,7 +217,6 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
             all_zero = false;
         }
     }
-    dev_sem->give();
 
     uint16_t crc_read = prom[7]&0xf;
     prom[7] &= 0xff00;
@@ -257,12 +252,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 +499,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