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

AP_BoardConfig: detect Holybro 6X Rev6

This commit is contained in:
Andrew Tridgell 2023-10-24 14:52:39 +11:00
parent cd3eab3c5d
commit b86774789b
2 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_BoardConfig/AP_BoardConfig.h
View File

@ -67,6 +67,7 @@ public:
PX4_BOARD_FMUV6 = 39,
FMUV6_BOARD_HOLYBRO_6X = 40,
FMUV6_BOARD_CUAV_6X = 41,
FMUV6_BOARD_HOLYBRO_6X_REV6 = 42,
PX4_BOARD_OLDDRIVERS = 100,
};

21
libraries/AP_BoardConfig/board_drivers.cpp
View File

@ -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;
@ -261,8 +256,9 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
#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