Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: improved robustness of BMM150 detection code

Browse Source 
cope with bus errors on startup
This commit is contained in:
Andrew Tridgell 2017-06-14 17:03:30 +10:00
parent 68460144df
commit f3d995a716
1 changed files with 49 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

@ -101,10 +101,27 @@ bool AP_Compass_BMM150::_load_trim_values()
le16_t dig_z3; le16_t dig_z3;
int8_t dig_xy2; int8_t dig_xy2;
uint8_t dig_xy1; uint8_t dig_xy1;
} PACKED trim_registers; } PACKED trim_registers, trim_registers2;
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers, // read the registers twice to confirm we have the right
sizeof(trim_registers))) { // values. There is no CRC in the registers and these values are
// vital to correct operation
int8_t tries = 4;
while (tries--) {
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,
sizeof(trim_registers))) {
continue;
}
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers2,
sizeof(trim_registers))) {
continue;
}
if (memcmp(&trim_registers, &trim_registers2, sizeof(trim_registers)) == 0) {
break;
}
}
if (-1 == tries) {
hal.console->printf("BMM150: Failed to load trim registers\n");
return false; return false;
} }
@ -135,30 +152,40 @@ bool AP_Compass_BMM150::init()
// 10 retries for init // 10 retries for init
_dev->set_retries(10); _dev->set_retries(10);
/* Do a soft reset */
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
if (!ret) {
goto bus_error;
}
hal.scheduler->delay(2);
// use checked registers to cope with bus errors
_dev->setup_checked_registers(4); _dev->setup_checked_registers(4);
int8_t boot_tries = 4;
while (boot_tries--) {
/* Do a soft reset */
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
hal.scheduler->delay(2);
if (!ret) {
continue;
}
/* Change power state from suspend mode to sleep mode */ /* Change power state from suspend mode to sleep mode */
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true); ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);
if (!ret) { hal.scheduler->delay(2);
goto bus_error; if (!ret) {
} continue;
hal.scheduler->delay(2); }
ret = _dev->read_registers(CHIP_ID_REG, &val, 1); ret = _dev->read_registers(CHIP_ID_REG, &val, 1);
if (!ret) { if (!ret) {
goto bus_error; continue;
}
if (val == CHIP_ID_VAL) {
// found it
break;
}
if (boot_tries == 0) {
hal.console->printf("BMM150: Wrong chip ID 0x%02x should be 0x%02x\n", val, CHIP_ID_VAL);
}
} }
if (val != CHIP_ID_VAL) { if (-1 == boot_tries) {
hal.console->printf("BMM150: Wrong id\n"); goto bus_error;
goto fail;
} }
ret = _load_trim_values(); ret = _load_trim_values();
@ -209,7 +236,6 @@ bool AP_Compass_BMM150::init()
bus_error: bus_error:
hal.console->printf("BMM150: Bus communication error\n"); hal.console->printf("BMM150: Bus communication error\n");
fail:
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
return false; return false;
} }