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; };