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, PX4_BOARD_FMUV6 = 39,
FMUV6_BOARD_HOLYBRO_6X = 40, FMUV6_BOARD_HOLYBRO_6X = 40,
FMUV6_BOARD_CUAV_6X = 41, FMUV6_BOARD_CUAV_6X = 41,
FMUV6_BOARD_HOLYBRO_6X_REV6 = 42,
PX4_BOARD_OLDDRIVERS = 100, PX4_BOARD_OLDDRIVERS = 100,
}; };

23
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_PCNC1:
case PX4_BOARD_MINDPXV2: case PX4_BOARD_MINDPXV2:
case FMUV6_BOARD_HOLYBRO_6X: case FMUV6_BOARD_HOLYBRO_6X:
case FMUV6_BOARD_HOLYBRO_6X_REV6:
case FMUV6_BOARD_CUAV_6X: case FMUV6_BOARD_CUAV_6X:
break; break;
default: default:
@ -129,17 +130,15 @@ bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uin
return false; return false;
} }
dev->set_read_flag(read_flag); dev->set_read_flag(read_flag);
dev->get_semaphore()->take_blocking(); WITH_SEMAPHORE(dev->get_semaphore());
dev->set_speed(AP_HAL::Device::SPEED_LOW); 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
hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum); hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
#endif #endif
dev->get_semaphore()->give();
return false; return false;
} }
dev->get_semaphore()->give();
#if SPI_PROBE_DEBUG #if SPI_PROBE_DEBUG
hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v); hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
#endif #endif
@ -161,7 +160,7 @@ bool AP_BoardConfig::spi_check_register_inv2(const char *devname, uint8_t regnum
return false; return false;
} }
dev->set_read_flag(read_flag); dev->set_read_flag(read_flag);
dev->get_semaphore()->take_blocking(); WITH_SEMAPHORE(dev->get_semaphore());
dev->set_speed(AP_HAL::Device::SPEED_LOW); dev->set_speed(AP_HAL::Device::SPEED_LOW);
uint8_t v; uint8_t v;
// select bank 0 for who am i // 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 #if SPI_PROBE_DEBUG
hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum); hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
#endif #endif
dev->get_semaphore()->give();
return false; return false;
} }
dev->get_semaphore()->give();
#if SPI_PROBE_DEBUG #if SPI_PROBE_DEBUG
hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v); hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
#endif #endif
@ -195,7 +192,7 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
if (!dev_sem) { if (!dev_sem) {
return false; return false;
} }
dev_sem->take_blocking(); WITH_SEMAPHORE(dev_sem);
static const uint8_t CMD_MS56XX_RESET = 0x1E; static const uint8_t CMD_MS56XX_RESET = 0x1E;
static const uint8_t CMD_MS56XX_PROM = 0xA0; 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); const uint8_t reg = CMD_MS56XX_PROM + (i << 1);
uint8_t val[2]; uint8_t val[2];
if (!dev->transfer(&reg, 1, val, sizeof(val))) { if (!dev->transfer(&reg, 1, val, sizeof(val))) {
dev_sem->give();
#if SPI_PROBE_DEBUG #if SPI_PROBE_DEBUG
hal.console->printf("%s: transfer fail\n", devname); hal.console->printf("%s: transfer fail\n", devname);
#endif #endif
@ -221,7 +217,6 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
all_zero = false; all_zero = false;
} }
} }
dev_sem->give();
uint16_t crc_read = prom[7]&0xf; uint16_t crc_read = prom[7]&0xf;
prom[7] &= 0xff00; prom[7] &= 0xff00;
@ -257,12 +252,13 @@ bool AP_BoardConfig::check_ms5611(const char* devname) {
#define INV2_WHOAMI_ICM20649 0xE1 #define INV2_WHOAMI_ICM20649 0xE1
#define INV3REG_WHOAMI 0x75 #define INV3REG_WHOAMI 0x75
#define INV3REG_456_WHOAMI 0x72 #define INV3REG_456_WHOAMI 0x72
#define INV3_WHOAMI_ICM42688 0x47 #define INV3_WHOAMI_ICM42688 0x47
#define INV3_WHOAMI_ICM42670 0x67 #define INV3_WHOAMI_ICM42670 0x67
#define INV3_WHOAMI_ICM45686 0xE9 #define INV3_WHOAMI_ICM45686 0xE9
#define INV3_WHOAMI_IIM42652 0x6f
/* /*
validation of the board type 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); state.board_type.set_and_notify(FMUV6_BOARD_CUAV_6X);
DEV_PRINTF("Detected CUAV 6X\n"); DEV_PRINTF("Detected CUAV 6X\n");
AP_Param::load_defaults_file("@ROMFS/param/CUAV_V6X_defaults.parm", false); 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 #endif