From a1f363aa00aa248ae6e8b7eba89b14278750bdb4 Mon Sep 17 00:00:00 2001
From: Niklas Hauser <niklas@auterion.com>
Date: Fri, 14 Feb 2025 15:29:15 +0100
Subject: [PATCH] [IST8310] Reset all I2C addresses on startup

Reading the WAI register is unreliable as the chip sometimes returns
wrong values or boots with the wrong I2C address. This can be fixed by
sending the software reset command to all four possible I2C addresses.
---
 .../magnetometer/isentek/ist8310/IST8310.cpp  | 45 ++++++++++---------
 .../magnetometer/isentek/ist8310/IST8310.hpp  |  2 +-
 2 files changed, 25 insertions(+), 22 deletions(-)

diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
index e5baf14ac5..e6c78d77f9 100644
--- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
+++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
@@ -85,31 +85,31 @@ void IST8310::print_status()
 
 int IST8310::probe()
 {
-	uint8_t id = RegisterRead(Register::WAI);
+	// Reading the WAI register is not always reliable, it can return 0xff or
+	// other values if the sensor has been powered up in a certain way. In
+	// addition, the I2C address is not always correct, sometimes it boots with
+	// 0x0C rather than 0x0E.
+	const auto start_time = hrt_absolute_time();
 
-	if (id != Device_ID) {
-		DEVICE_DEBUG("unexpected WAI 0x%02x", id);
+	while (hrt_elapsed_time(&start_time) < 50_ms) {
+		set_device_address(I2C_ADDRESS_DEFAULT);
+		const int WAI = RegisterRead(Register::WAI);
 
-		// Apparently, the IST8310's WHOAMI register is writeable. Presumably,
-		// this can get corrupted by bus noise. It is only reset if powered off
-		// for 30s or by a reset.
-		RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST);
-
-		auto start_time = hrt_absolute_time();
-
-		while (hrt_elapsed_time(&start_time) < 50_ms) {
-			px4_usleep(10'000);
-			id = RegisterRead(Register::WAI);
-
-			if (id == Device_ID) {
-				return PX4_OK;
-			}
+		if (WAI == Device_ID) {
+			// Device has the right I2C address and register content
+			return PX4_OK;
 		}
 
-		return PX4_ERROR;
+		// send reset command to all four possible addresses
+		for (uint8_t addr = 0x0C; addr <= 0x0F; addr++) {
+			set_device_address(addr);
+			RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST);
+		}
+
+		px4_usleep(10'000);
 	}
 
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 void IST8310::RunImpl()
@@ -290,11 +290,14 @@ bool IST8310::RegisterCheck(const register_config_t &reg_cfg)
 	return success;
 }
 
-uint8_t IST8310::RegisterRead(Register reg)
+int IST8310::RegisterRead(Register reg)
 {
 	const uint8_t cmd = static_cast<uint8_t>(reg);
 	uint8_t buffer{};
-	transfer(&cmd, 1, &buffer, 1);
+	const int ret = transfer(&cmd, 1, &buffer, 1);
+
+	if (ret != OK) { return -1; }
+
 	return buffer;
 }
 
diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp
index a251f49cf0..5762377cb6 100644
--- a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp
+++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp
@@ -79,7 +79,7 @@ private:
 
 	bool RegisterCheck(const register_config_t &reg_cfg);
 
-	uint8_t RegisterRead(Register reg);
+	int RegisterRead(Register reg);
 	void RegisterWrite(Register reg, uint8_t value);
 	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);