Add IMU timeout detection to SoftFusionSensor

SlimeVR · Jan 13, 2025 · bd987c3 · bd987c3
1 parent 6942e48
commit bd987c3
Showing 1 changed file with 17 additions and 0 deletions.
diff --git 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;
 };