From e66683a059e4f4dc6535604cae7699f080ecca51 Mon Sep 17 00:00:00 2001
From: Bruce Meagher <bmeagher@glpsoftware.com>
Date: Wed, 4 May 2022 06:40:58 -0700
Subject: [PATCH] Fixed rm3100 self-test bug

---
 src/drivers/magnetometer/rm3100/rm3100.cpp | 55 +++++++++++++---------
 1 file changed, 32 insertions(+), 23 deletions(-)

diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp
index b360c394c1..2bc5ebbf37 100644
--- a/src/drivers/magnetometer/rm3100/rm3100.cpp
+++ b/src/drivers/magnetometer/rm3100/rm3100.cpp
@@ -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);