Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BoardConfig: added board detection for FMUv6 

support Holybro-6X and CUAV-6X for now, using IMU signatures
This commit is contained in:
Andrew Tridgell 2022-07-19 08:30:56 +10:00
parent d4bfcf80bd
commit b50d405f91
2 changed files with 25 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_BoardConfig/AP_BoardConfig.h
View File

@ -96,6 +96,8 @@ public:
VRX_BOARD_CORE10 = 36, VRX_BOARD_CORE10 = 36,
VRX_BOARD_BRAIN54 = 38, VRX_BOARD_BRAIN54 = 38,
PX4_BOARD_FMUV6 = 39, PX4_BOARD_FMUV6 = 39,
FMUV6_BOARD_HOLYBRO_6X = 40,
FMUV6_BOARD_CUAV_6X = 41,
PX4_BOARD_OLDDRIVERS = 100, 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); 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 validate_board_type(void);
void board_autodetect(void); void board_autodetect(void);
void detect_fmuv6_variant(void);
bool check_ms5611(const char* devname); bool check_ms5611(const char* devname);
#endif // AP_FEATURE_BOARD_DETECT #endif // AP_FEATURE_BOARD_DETECT

24
libraries/AP_BoardConfig/board_drivers.cpp
View File

@ -117,6 +117,8 @@ void AP_BoardConfig::board_setup_drivers(void)
case PX4_BOARD_PIXHAWK_PRO: case PX4_BOARD_PIXHAWK_PRO:
case PX4_BOARD_PCNC1: case PX4_BOARD_PCNC1:
case PX4_BOARD_MINDPXV2: case PX4_BOARD_MINDPXV2:
case FMUV6_BOARD_HOLYBRO_6X:
case FMUV6_BOARD_CUAV_6X:
break; break;
default: default:
config_error("Unknown board type"); config_error("Unknown board type");
@ -370,8 +372,7 @@ void AP_BoardConfig::board_autodetect(void)
state.board_type.set_and_notify(PX4_BOARD_FMUV5); state.board_type.set_and_notify(PX4_BOARD_FMUV5);
DEV_PRINTF("Detected FMUv5\n"); DEV_PRINTF("Detected FMUv5\n");
#elif defined(HAL_CHIBIOS_ARCH_FMUV6) #elif defined(HAL_CHIBIOS_ARCH_FMUV6)
state.board_type.set_and_notify(PX4_BOARD_FMUV5); detect_fmuv6_variant();
DEV_PRINTF("Detected FMUv6\n");
#elif defined(HAL_CHIBIOS_ARCH_BRAINV51) #elif defined(HAL_CHIBIOS_ARCH_BRAINV51)
state.board_type.set_and_notify(VRX_BOARD_BRAIN51); state.board_type.set_and_notify(VRX_BOARD_BRAIN51);
DEV_PRINTF("Detected VR Brain 5.1\n"); DEV_PRINTF("Detected VR Brain 5.1\n");
@ -486,3 +487,22 @@ void AP_BoardConfig::board_setup()
#endif #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