From a6159604e59be1dc0a77366dbd6ec723310d9039 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 2 Aug 2022 08:12:08 +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 | 10 ++++++---- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 34297de10d..02be5f5d97 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 6407526312..fd0c0fb2c0 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -134,6 +134,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"); @@ -400,8 +402,7 @@ void AP_BoardConfig::board_autodetect(void) state.board_type.set_and_notify(PX4_BOARD_FMUV5); hal.console->printf("Detected FMUv5\n"); #elif defined(HAL_CHIBIOS_ARCH_FMUV6) - state.board_type.set_and_notify(PX4_BOARD_FMUV5); - hal.console->printf("Detected FMUv6\n"); + detect_fmuv6_variant(); #elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(HAL_CHIBIOS_ARCH_BRAINV51) state.board_type.set_and_notify(VRX_BOARD_BRAIN51); hal.console->printf("Detected VR Brain 5.1\n"); @@ -522,6 +523,7 @@ void AP_BoardConfig::board_setup() #endif } + #ifdef HAL_CHIBIOS_ARCH_FMUV6 #define BMI088REG_CHIPID 0x00 @@ -537,12 +539,12 @@ void AP_BoardConfig::detect_fmuv6_variant() 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"); + hal.console->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("bmi088_g", BMI088REG_CHIPID, CHIPID_BMI088_G))) { state.board_type.set_and_notify(FMUV6_BOARD_CUAV_6X); - DEV_PRINTF("Detected CUAV 6X\n"); + hal.console->printf("Detected CUAV 6X\n"); } }