dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fixed rm3100 self-test bug

This commit is contained in:
Bruce Meagher 2022-05-04 06:40:58 -07:00 committed by Daniel Agar
parent 1a620b450d
commit e66683a059
1 changed files with 32 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
src/drivers/magnetometer/rm3100/rm3100.cpp
View File

@ -63,13 +63,21 @@ int RM3100::self_test()
{
bool complete = false;
uint8_t cmd = (CMM_DEFAULT | POLLING_MODE);
// Set the default command mode and enable polling (not continuous mode)
uint8_t cmd = (CMM_DEFAULT & ~CONTINUOUS_MODE);
int ret = _interface->write(ADDR_CMM, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
cmd = HSHAKE_NO_DRDY_CLEAR;
ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
// Configure sensor to execute BIST upon receipt of a POLL command
cmd = BIST_SELFTEST;
ret = _interface->write(ADDR_BIST, &cmd, 1);
@ -78,27 +86,20 @@ int RM3100::self_test()
return ret;
}
/* Perform test procedure until a valid result is obtained or test times out */
// Poll to start the self test
cmd = POLL_XYZ;
ret = _interface->write(ADDR_POLL, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
// Perform test procedure until a valid result is obtained or test times out
const hrt_abstime t_start = hrt_absolute_time();
while ((hrt_absolute_time() - t_start) < BIST_DUR_USEC) {
// Re-disable DRDY clear
cmd = HSHAKE_NO_DRDY_CLEAR;
ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
// Poll for a measurement
cmd = POLL_XYZ;
ret = _interface->write(ADDR_POLL, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
uint8_t status = 0;
ret = _interface->read(ADDR_STATUS, &status, 1);
@ -119,8 +120,16 @@ int RM3100::self_test()
if (cmd & BIST_STE) {
complete = true;
// If the test passed, disable self-test mode by clearing the STE bit
if (cmd & BIST_XYZ_OK) {
// If the x, y, or z LR oscillators malfunctioned then the self test failed.
if ((cmd & BIST_XYZ_OK) ^ BIST_XYZ_OK) {
PX4_ERR("built-in self test failed: 0x%2X x:%s y:%s z:%s", cmd,
cmd & 0x10 ? "Pass" : "Fail",
cmd & 0x20 ? "Pass" : "Fail",
cmd & 0x40 ? "Pass" : "Fail" );
return PX4_ERROR;
} else {
// The test passed, disable self-test mode by clearing the STE bit
cmd = 0;
ret = _interface->write(ADDR_BIST, &cmd, 1);
@ -129,9 +138,6 @@ int RM3100::self_test()
}
return PX4_OK;
} else {
PX4_ERR("built-in self test failed");
}
}
}
@ -261,6 +267,9 @@ int RM3100::set_default_register_values()
cmd[1] = CCZ_DEFAULT_LSB;
_interface->write(ADDR_CCZ, cmd, 2);
cmd[0] = HSHAKE_DEFAULT;
_interface->write(ADDR_HSHAKE, cmd, 1);
cmd[0] = CMM_DEFAULT;
_interface->write(ADDR_CMM, cmd, 1);