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>

src/drivers/magnetometer/rm3100/rm3100.cpp

@@ -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,33 +83,76 @@ 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();
+
+	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;
 		}
 
-	/* Now wait for status register */
-	usleep(RM3100_CONVERSION_INTERVAL);
+		/* Poll for a measurement */
+		cmd = POLL_XYZ;
+		ret = _interface->write(ADDR_POLL, &cmd, 1);
 
-	if (check_measurement() != PX4_OK) {
-		return -1;;
+		if (ret != PX4_OK) {
+			return ret;
 		}
 
-	/* Now check BIST register to see whether self test is ok or not*/
+		/* 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;
 			}
 
-	ret = !((cmd & BIST_XYZ_OK) == BIST_XYZ_OK);
+			/* 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");
+				}
+			}
+		}
+	}
+
+	if (!complete) {
+		PX4_ERR("BIST incomplete");
+	}
+
+	return pass;
+}
+
 int RM3100::check_measurement()
 {
 	uint8_t status = 0;
@@ -122,7 +162,7 @@ int RM3100::check_measurement()
 		return ret;
 	}
 
-	return !((status & STATUS_DRDY) == STATUS_DRDY) ;
+	return !(status & STATUS_DRDY);
 }
 
 int RM3100::collect()

src/drivers/magnetometer/rm3100/rm3100.h

@@ -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