fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/magnetometer/bosch/bmm150: init and self test improvements

Browse Source 
- if self test fails consistently proceed with startup, but report failure
This commit is contained in:
Daniel Agar
2022-01-24 16:16:03 -05:00
parent 8217e0f335
commit 8d03e71c16
1 changed files with 45 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/magnetometer/bosch/bmm150/BMM150.cpp
+45 -20
View File
@@ -92,11 +92,11 @@ int BMM150::probe()
PX4_DEBUG("POWER_CONTROL: 0x%02hhX, CHIP_ID: 0x%02hhX", POWER_CONTROL, CHIP_ID); PX4_DEBUG("POWER_CONTROL: 0x%02hhX, CHIP_ID: 0x%02hhX", POWER_CONTROL, CHIP_ID);
// either power control bit is set and chip ID can be read, or both registers are 0x00 if (CHIP_ID == chip_identification_number) {
if ((POWER_CONTROL & POWER_CONTROL_BIT::PowerControl) && (CHIP_ID == chip_identification_number)) {
return PX4_OK; return PX4_OK;
} else if ((POWER_CONTROL == 0) && (CHIP_ID == 0)) { } else if ((CHIP_ID == 0) && !(POWER_CONTROL & POWER_CONTROL_BIT::PowerControl)) {
// in suspend Chip ID read (register 0x40) returns “0x00” (I²C) or high-Z (SPI).
return PX4_OK; return PX4_OK;
} }
} }
@@ -185,14 +185,15 @@ void BMM150::RunImpl()
_failure_count = 0; _failure_count = 0;
_state = STATE::WAIT_FOR_RESET; _state = STATE::WAIT_FOR_RESET;
perf_count(_reset_perf); perf_count(_reset_perf);
ScheduleDelayed(3_ms); // 3.0 ms start-up time from suspend to sleep ScheduleDelayed(10_ms); // 3.0 ms start-up time from suspend to sleep
break; break;
case STATE::WAIT_FOR_RESET: case STATE::WAIT_FOR_RESET:
// Soft reset always brings the device into sleep mode (power off -> suspend -> sleep) // Soft reset always brings the device into sleep mode (power off -> suspend -> sleep)
if ((RegisterRead(Register::CHIP_ID) == chip_identification_number) if ((RegisterRead(Register::CHIP_ID) == chip_identification_number)
&& (RegisterRead(Register::POWER_CONTROL) == POWER_CONTROL_BIT::PowerControl) && (RegisterRead(Register::POWER_CONTROL) & POWER_CONTROL_BIT::PowerControl)
&& !(RegisterRead(Register::POWER_CONTROL) & POWER_CONTROL_BIT::SoftReset)
&& (RegisterRead(Register::OP_MODE) == OP_MODE_BIT::Opmode_Sleep)) { && (RegisterRead(Register::OP_MODE) == OP_MODE_BIT::Opmode_Sleep)) {
// if reset succeeded then start self test // if reset succeeded then start self test
@@ -220,23 +221,46 @@ void BMM150::RunImpl()
// After performing self test OpMode "Self test" bit is set to 0 // After performing self test OpMode "Self test" bit is set to 0
const bool opmode_self_test_cleared = ((RegisterRead(Register::OP_MODE) & OP_MODE_BIT::Self_Test) == 0); const bool opmode_self_test_cleared = ((RegisterRead(Register::OP_MODE) & OP_MODE_BIT::Self_Test) == 0);
// When self-test is successful, the corresponding self-test result bits are set if (opmode_self_test_cleared) {
// “X-Self-Test” register 0x42 bit0 // When self-test is successful, the corresponding self-test result bits are set
// “Y-Self-Test” register 0x44 bit0 // “X-Self-Test” register 0x42 bit0
// “Z-Self-Test” register 0x46 bit0 // “Y-Self-Test” register 0x44 bit0
const bool x_success = RegisterRead(Register::DATAX_LSB) & Bit0; // “Z-Self-Test” register 0x46 bit0
const bool y_success = RegisterRead(Register::DATAY_LSB) & Bit0; const bool x_success = RegisterRead(Register::DATAX_LSB) & Bit0;
const bool z_success = RegisterRead(Register::DATAZ_LSB) & Bit0; const bool y_success = RegisterRead(Register::DATAY_LSB) & Bit0;
const bool z_success = RegisterRead(Register::DATAZ_LSB) & Bit0;
if (opmode_self_test_cleared && (!x_success || !y_success || !z_success)) { if (x_success && y_success && z_success) {
PX4_DEBUG("self test failed, resetting"); _state = STATE::READ_TRIM;
perf_count(_self_test_failed_perf); ScheduleDelayed(10_ms);
_state = STATE::RESET;
ScheduleDelayed(1000_ms); } else {
if (perf_event_count(_self_test_failed_perf) >= 5) {
PX4_ERR("self test still failing after 5 attempts");
// reluctantly proceed
_state = STATE::READ_TRIM;
ScheduleDelayed(10_ms);
} else {
PX4_ERR("self test failed, resetting");
perf_count(_self_test_failed_perf);
_state = STATE::RESET;
ScheduleDelayed(1_s);
}
}
} else { } else {
_state = STATE::READ_TRIM; if (hrt_elapsed_time(&_reset_timestamp) < 3_s) {
ScheduleDelayed(1_ms); // self test not complete, check again in 100 milliseconds
_state = STATE::SELF_TEST_CHECK;
ScheduleDelayed(100_ms);
} else {
// full reset
_state = STATE::RESET;
ScheduleDelayed(10_ms);
}
} }
} }
@@ -353,7 +377,8 @@ void BMM150::RunImpl()
perf_count(_overflow_perf); perf_count(_overflow_perf);
} else { } else {
_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); _px4_mag.set_error_count(perf_event_count(_bad_register_perf)
+ perf_event_count(_bad_transfer_perf) + perf_event_count(_self_test_failed_perf));
_px4_mag.update(now, compensate_x(x, rhall), compensate_y(y, rhall), compensate_z(z, rhall)); _px4_mag.update(now, compensate_x(x, rhall), compensate_y(y, rhall), compensate_z(z, rhall));
success = true; success = true;