drivers/magnetometer/rm3100: self-test procedure revision (#19207)

* Adds retry behavior and additional checks to RM3100 BIST

Signed-off-by: Charles Cross <charles@missionrobotics.us>
This commit is contained in:
Charles Cross 2022-02-21 11:27:42 -05:00 committed by GitHub
parent 960003a86a
commit 699f34ba83
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 71 additions and 27 deletions

View File

@ -70,15 +70,12 @@ RM3100::~RM3100()
int RM3100::self_test()
{
/* Chances are that a poll event was triggered, so wait for conversion and read registers in order to clear DRDY bit */
usleep(RM3100_CONVERSION_INTERVAL);
collect();
/* Fail if calibration is not good */
int ret = 0;
uint8_t cmd = 0;
bool complete = false;
int pass = PX4_ERROR;
/* Configure mag into self test mode */
/* Configure sensor to execute BIST upon receipt of a POLL command */
cmd = BIST_SELFTEST;
ret = _interface->write(ADDR_BIST, &cmd, 1);
@ -86,31 +83,74 @@ int RM3100::self_test()
return ret;
}
/* Now we need to write to POLL to launch self test */
cmd = POLL_XYZ;
ret = _interface->write(ADDR_POLL, &cmd, 1);
/* Perform test procedure until a valid result is obtained or test times out */
const hrt_abstime t_start = hrt_absolute_time();
if (ret != PX4_OK) {
return ret;
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;
}
/* If the DRDY bit in the status register is set, BIST should be complete */
if (!check_measurement()) {
/* Check BIST register to evaluate the test result*/
ret = _interface->read(ADDR_BIST, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
/* The test results are not valid if STE is not set. In this case, we try again */
if (cmd & BIST_STE) {
complete = true;
/* If the test passed, disable self-test mode by clearing the STE bit */
ret = !(cmd & BIST_XYZ_OK);
if (!ret) {
cmd = 0;
ret = _interface->write(ADDR_BIST, &cmd, 1);
if (ret != PX4_OK) {
PX4_ERR("Failed to disable BIST");
}
/* Re-enable DRDY clear upon register writes and measurements */
cmd = HSHAKE_DEFAULT;
ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
pass = PX4_OK;
break;
} else {
PX4_ERR("BIST failed");
}
}
}
}
/* Now wait for status register */
usleep(RM3100_CONVERSION_INTERVAL);
if (check_measurement() != PX4_OK) {
return -1;;
if (!complete) {
PX4_ERR("BIST incomplete");
}
/* Now check BIST register to see whether self test is ok or not*/
ret = _interface->read(ADDR_BIST, &cmd, 1);
if (ret != PX4_OK) {
return ret;
}
ret = !((cmd & BIST_XYZ_OK) == BIST_XYZ_OK);
return ret;
return pass;
}
int RM3100::check_measurement()
@ -122,7 +162,7 @@ int RM3100::check_measurement()
return ret;
}
return !((status & STATUS_DRDY) == STATUS_DRDY) ;
return !(status & STATUS_DRDY);
}
int RM3100::collect()

View File

@ -82,6 +82,10 @@
#define BIST_SELFTEST 0x8F
#define BIST_DEFAULT 0x00
#define BIST_XYZ_OK ((1 << 4) | (1 << 5) | (1 << 6))
#define BIST_STE (1 << 7)
#define BIST_DUR_USEC (2*RM3100_CONVERSION_INTERVAL)
#define HSHAKE_DEFAULT (0x0B)
#define HSHAKE_NO_DRDY_CLEAR (0x08)
#define STATUS_DRDY (1 << 7)
#define POLL_XYZ 0x70
#define RM3100_REVID 0x22