diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index e61e5f4d44..bb082a8531 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -271,6 +271,7 @@ bool AP_BoardConfig::check_ms5611(const char* devname) { #define INV3REG_WHOAMI 0x75 #define INV3_WHOAMI_ICM42688 0x47 +#define INV3_WHOAMI_ICM42670 0x67 /* validation of the board type @@ -489,17 +490,24 @@ void AP_BoardConfig::board_setup() #ifdef HAL_CHIBIOS_ARCH_FMUV6 + +#define BMI088REG_CHIPID 0x00 +#define CHIPID_BMI088_G 0x0F + /* 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))) { + if (((spi_check_register_inv2("icm20649", INV2REG_WHOAMI, INV2_WHOAMI_ICM20649) || + spi_check_register("bmi088_g", BMI088REG_CHIPID, CHIPID_BMI088_G)) && // alternative config + spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688) && + spi_check_register("icm42670", INV3REG_WHOAMI, INV3_WHOAMI_ICM42670))) { 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))) { + spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688) && + spi_check_register("bmi088_g", BMI088REG_CHIPID, CHIPID_BMI088_G))) { state.board_type.set_and_notify(FMUV6_BOARD_CUAV_6X); DEV_PRINTF("Detected CUAV 6X\n"); }