From bd987c3531f8b3978f23350c316d9e51924b538a Mon Sep 17 00:00:00 2001
From: pembem22 <24252798+pembem22@users.noreply.github.com>
Date: Mon, 13 Jan 2025 19:57:55 +0200
Subject: [PATCH] Add IMU timeout detection to `SoftFusionSensor`

---
 src/sensors/softfusion/softfusionsensor.h | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h
index 857b24fe..e8ea3968 100644
--- a/src/sensors/softfusion/softfusionsensor.h
+++ b/src/sensors/softfusion/softfusionsensor.h
@@ -199,6 +199,20 @@ class SoftFusionSensor : public Sensor {
 		, m_sensor(I2CImpl(i2cAddress), m_Logger) {}
 	~SoftFusionSensor() {}
 
+	void checkSensorTimeout() {
+		uint32_t now = micros();
+		if (m_lastRotationUpdate + 2_000_000 < now) {
+			working = false;
+			m_status = SensorStatus::SENSOR_ERROR;
+			m_Logger.error(
+				"Sensor timeout I2C Address 0x%02x delaytime: %d ms",
+				addr,
+				now - m_lastRotationUpdate
+			);
+			networkConnection.sendSensorError(this->sensorId, 1);
+		}
+	}
+
 	void motionLoop() override final {
 		sendTempIfNeeded();
 
@@ -218,9 +232,11 @@ class SoftFusionSensor : public Sensor {
 			);
 			optimistic_yield(100);
 			if (!m_fusion.isUpdated()) {
+				checkSensorTimeout();
 				return;
 			}
 			hadData = true;
+			m_lastRotationUpdate = now;
 			m_fusion.clearUpdated();
 		}
 
@@ -615,6 +631,7 @@ class SoftFusionSensor : public Sensor {
 
 	SensorStatus m_status = SensorStatus::SENSOR_OFFLINE;
 	uint32_t m_lastPollTime = micros();
+	uint32_t m_lastRotationUpdate = 0;
 	uint32_t m_lastRotationPacketSent = 0;
 	uint32_t m_lastTemperaturePacketSent = 0;
 };