Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_BoardConfig: do bank selection when checking Invensensev2 Sensors

Browse Source
This commit is contained in:
bugobliterator 2020-09-13 20:53:14 +05:30 committed by Andrew Tridgell
parent ab1d0d53f3
commit b390f402b7
2 changed files with 39 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_BoardConfig/AP_BoardConfig.h
View File

@ -203,6 +203,7 @@ private:
void board_setup_drivers(void); void board_setup_drivers(void);
bool spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag = 0x80); bool spi_check_register(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);
bool check_ms5611(const char* devname); bool check_ms5611(const char* devname);

40
libraries/AP_BoardConfig/board_drivers.cpp
View File

@ -137,6 +137,7 @@ bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uin
} }
dev->set_read_flag(read_flag); dev->set_read_flag(read_flag);
dev->get_semaphore()->take_blocking(); dev->get_semaphore()->take_blocking();
dev->set_speed(AP_HAL::Device::SPEED_LOW);
uint8_t v; uint8_t v;
if (!dev->read_registers(regnum, &v, 1)) { if (!dev->read_registers(regnum, &v, 1)) {
#if SPI_PROBE_DEBUG #if SPI_PROBE_DEBUG
@ -152,6 +153,40 @@ bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uin
return v == value; return v == value;
} }
#define INV2REG_BANK_SEL 0x7F
/*
check a SPI device for a register value
*/
bool AP_BoardConfig::spi_check_register_inv2(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag)
{
auto dev = hal.spi->get_device(devname);
if (!dev) {
#if SPI_PROBE_DEBUG
hal.console->printf("%s: no device\n", devname);
#endif
return false;
}
dev->set_read_flag(read_flag);
dev->get_semaphore()->take_blocking();
dev->set_speed(AP_HAL::Device::SPEED_LOW);
uint8_t v;
// select bank 0 for who am i
dev->write_register(INV2REG_BANK_SEL, 0, false);
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
return v == value;
}
#if defined(HAL_VALIDATE_BOARD) #if defined(HAL_VALIDATE_BOARD)
bool AP_BoardConfig::check_ms5611(const char* devname) { bool AP_BoardConfig::check_ms5611(const char* devname) {
auto dev = hal.spi->get_device(devname); auto dev = hal.spi->get_device(devname);
@ -224,6 +259,7 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
#define LSM_WHOAMI_L3GD20 0xd4 #define LSM_WHOAMI_L3GD20 0xd4
#define INV2REG_WHOAMI 0x00 #define INV2REG_WHOAMI 0x00
#define INV2_WHOAMI_ICM20948 0xEA #define INV2_WHOAMI_ICM20948 0xEA
#define INV2_WHOAMI_ICM20649 0xE1 #define INV2_WHOAMI_ICM20649 0xE1
@ -245,7 +281,7 @@ void AP_BoardConfig::validate_board_type(void)
spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) || spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) ||
spi_check_register("icm20602_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) && spi_check_register("icm20602_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
(spi_check_register("lsm9ds0_ext_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) || (spi_check_register("lsm9ds0_ext_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) ||
spi_check_register("icm20948_ext", INV2REG_WHOAMI, INV2_WHOAMI_ICM20948))) { spi_check_register_inv2("icm20948_ext", INV2REG_WHOAMI, INV2_WHOAMI_ICM20948))) {
// Pixhawk2 has LSM303D and MPUxxxx on external bus. If we // Pixhawk2 has LSM303D and MPUxxxx on external bus. If we
// detect those, then force PIXHAWK2, even if the user has // detect those, then force PIXHAWK2, even if the user has
// configured for PIXHAWK1 // configured for PIXHAWK1
@ -298,7 +334,7 @@ void AP_BoardConfig::board_autodetect(void)
spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) || spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) ||
spi_check_register("icm20602_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) && spi_check_register("icm20602_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
(spi_check_register("lsm9ds0_ext_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) || (spi_check_register("lsm9ds0_ext_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) ||
spi_check_register("icm20948_ext", INV2REG_WHOAMI, INV2_WHOAMI_ICM20948))) { spi_check_register_inv2("icm20948_ext", INV2REG_WHOAMI, INV2_WHOAMI_ICM20948))) {
// Pixhawk2 has LSM303D and MPUxxxx on external bus // Pixhawk2 has LSM303D and MPUxxxx on external bus
state.board_type.set(PX4_BOARD_PIXHAWK2); state.board_type.set(PX4_BOARD_PIXHAWK2);
hal.console->printf("Detected PIXHAWK2\n"); hal.console->printf("Detected PIXHAWK2\n");