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 1/8] 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; }; From 2c01aad5f1562ad7c89ea527acdcdfab6252185e Mon Sep 17 00:00:00 2001 From: pembem22 <24252798+pembem22@users.noreply.github.com> Date: Mon, 13 Jan 2025 20:42:28 +0200 Subject: [PATCH 2/8] Early return, use constexpr --- src/sensors/softfusion/softfusionsensor.h | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index e8ea3968..7df501b0 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -201,16 +201,19 @@ class SoftFusionSensor : public Sensor { 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); + constexpr uint32_t sensorTimeoutMicros = 2e6; // 2 seconds + if (m_lastRotationUpdate + sensorTimeoutMicros > now) { + return; } + + 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 { From 1de877ba3e88d4391171ba37cb67256df0626a8e Mon Sep 17 00:00:00 2001 From: pembem22 <24252798+pembem22@users.noreply.github.com> Date: Tue, 14 Jan 2025 22:06:55 +0200 Subject: [PATCH 3/8] Define a sensor timeout error code --- src/network/packets.h | 2 ++ src/sensors/icm20948sensor.cpp | 2 +- src/sensors/softfusion/softfusionsensor.h | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/network/packets.h b/src/network/packets.h index f6c3d9b5..0f854904 100644 --- a/src/network/packets.h +++ b/src/network/packets.h @@ -67,4 +67,6 @@ #define PACKET_INSPECTION_DATATYPE_INT 1 #define PACKET_INSPECTION_DATATYPE_FLOAT 2 +#define PACKET_ERROR_SENSOR_TIMEOUT 1 + #endif // SLIMEVR_PACKETS_H_ diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index e3347683..7fe49d1e 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -329,7 +329,7 @@ void ICM20948Sensor::checkSensorTimeout() { addr, currenttime - lastData ); - networkConnection.sendSensorError(this->sensorId, 1); + networkConnection.sendSensorError(this->sensorId, PACKET_ERROR_SENSOR_TIMEOUT); lastData = currenttime; } } diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 7df501b0..b2861ccf 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -213,7 +213,7 @@ class SoftFusionSensor : public Sensor { addr, now - m_lastRotationUpdate ); - networkConnection.sendSensorError(this->sensorId, 1); + networkConnection.sendSensorError(this->sensorId, PACKET_ERROR_SENSOR_TIMEOUT); } void motionLoop() override final { From ebf436635c0e66209af941c09ebb32264ed54f49 Mon Sep 17 00:00:00 2001 From: pembem22 <24252798+pembem22@users.noreply.github.com> Date: Thu, 16 Jan 2025 19:39:35 +0200 Subject: [PATCH 4/8] Use enum instead of define --- src/network/packets.h | 4 +++- src/sensors/icm20948sensor.cpp | 5 ++++- src/sensors/softfusion/softfusionsensor.h | 5 ++++- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/network/packets.h b/src/network/packets.h index 0f854904..a52b4888 100644 --- a/src/network/packets.h +++ b/src/network/packets.h @@ -67,6 +67,8 @@ #define PACKET_INSPECTION_DATATYPE_INT 1 #define PACKET_INSPECTION_DATATYPE_FLOAT 2 -#define PACKET_ERROR_SENSOR_TIMEOUT 1 +enum class PacketErrorCode : uint8_t { + SENSOR_TIMEOUT = 1, +}; #endif // SLIMEVR_PACKETS_H_ diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 7fe49d1e..c4e466a4 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -329,7 +329,10 @@ void ICM20948Sensor::checkSensorTimeout() { addr, currenttime - lastData ); - networkConnection.sendSensorError(this->sensorId, PACKET_ERROR_SENSOR_TIMEOUT); + networkConnection.sendSensorError( + this->sensorId, + static_cast(PacketErrorCode::SENSOR_TIMEOUT) + ); lastData = currenttime; } } diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index b2861ccf..5cfe90c0 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -213,7 +213,10 @@ class SoftFusionSensor : public Sensor { addr, now - m_lastRotationUpdate ); - networkConnection.sendSensorError(this->sensorId, PACKET_ERROR_SENSOR_TIMEOUT); + networkConnection.sendSensorError( + this->sensorId, + static_cast(PacketErrorCode::SENSOR_TIMEOUT) + ); } void motionLoop() override final { From f4c2835c7f7627e4070ca7ce87856b1993768a9d Mon Sep 17 00:00:00 2001 From: pembem22 <24252798+pembem22@users.noreply.github.com> Date: Sun, 19 Jan 2025 20:02:55 +0200 Subject: [PATCH 5/8] Avoid false timeout on timer overflow --- src/sensors/softfusion/softfusionsensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 5cfe90c0..aeaf8fd1 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -202,7 +202,7 @@ class SoftFusionSensor : public Sensor { void checkSensorTimeout() { uint32_t now = micros(); constexpr uint32_t sensorTimeoutMicros = 2e6; // 2 seconds - if (m_lastRotationUpdate + sensorTimeoutMicros > now) { + if (now - m_lastRotationUpdate < sensorTimeoutMicros) { return; } From 6a535a61b72aa8f8a74dc60936f5bae64e74ea4f Mon Sep 17 00:00:00 2001 From: pembem22 <24252798+pembem22@users.noreply.github.com> Date: Sun, 19 Jan 2025 20:11:57 +0200 Subject: [PATCH 6/8] Revert "Avoid false timeout on timer overflow" This reverts commit f4c2835c7f7627e4070ca7ce87856b1993768a9d. --- src/sensors/softfusion/softfusionsensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index aeaf8fd1..5cfe90c0 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -202,7 +202,7 @@ class SoftFusionSensor : public Sensor { void checkSensorTimeout() { uint32_t now = micros(); constexpr uint32_t sensorTimeoutMicros = 2e6; // 2 seconds - if (now - m_lastRotationUpdate < sensorTimeoutMicros) { + if (m_lastRotationUpdate + sensorTimeoutMicros > now) { return; } From a1b55a55cf062a329c23f5bc41348f0f690ce8f1 Mon Sep 17 00:00:00 2001 From: pembem22 <24252798+pembem22@users.noreply.github.com> Date: Sun, 19 Jan 2025 20:14:50 +0200 Subject: [PATCH 7/8] Use millis instead of micros --- src/sensors/softfusion/softfusionsensor.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 5cfe90c0..a38b259d 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -200,9 +200,9 @@ class SoftFusionSensor : public Sensor { ~SoftFusionSensor() {} void checkSensorTimeout() { - uint32_t now = micros(); - constexpr uint32_t sensorTimeoutMicros = 2e6; // 2 seconds - if (m_lastRotationUpdate + sensorTimeoutMicros > now) { + uint32_t now = millis(); + constexpr uint32_t sensorTimeoutMillis = 2e3; // 2 seconds + if (m_lastRotationUpdateMillis + sensorTimeoutMillis > now) { return; } @@ -211,7 +211,7 @@ class SoftFusionSensor : public Sensor { m_Logger.error( "Sensor timeout I2C Address 0x%02x delaytime: %d ms", addr, - now - m_lastRotationUpdate + now - m_lastRotationUpdateMillis ); networkConnection.sendSensorError( this->sensorId, @@ -242,7 +242,7 @@ class SoftFusionSensor : public Sensor { return; } hadData = true; - m_lastRotationUpdate = now; + m_lastRotationUpdateMillis = millis(); m_fusion.clearUpdated(); } @@ -637,7 +637,7 @@ class SoftFusionSensor : public Sensor { SensorStatus m_status = SensorStatus::SENSOR_OFFLINE; uint32_t m_lastPollTime = micros(); - uint32_t m_lastRotationUpdate = 0; + uint32_t m_lastRotationUpdateMillis = 0; uint32_t m_lastRotationPacketSent = 0; uint32_t m_lastTemperaturePacketSent = 0; }; From 1c2680e4cd9a84e2ebd9eb23b6022083575976e5 Mon Sep 17 00:00:00 2001 From: pembem22 <24252798+pembem22@users.noreply.github.com> Date: Mon, 20 Jan 2025 21:58:11 +0200 Subject: [PATCH 8/8] Use SH-2 error codes --- src/network/packets.h | 8 +++++++- src/sensors/icm20948sensor.cpp | 2 +- src/sensors/softfusion/softfusionsensor.h | 2 +- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/network/packets.h b/src/network/packets.h index a52b4888..8bb2561d 100644 --- a/src/network/packets.h +++ b/src/network/packets.h @@ -67,8 +67,14 @@ #define PACKET_INSPECTION_DATATYPE_INT 1 #define PACKET_INSPECTION_DATATYPE_FLOAT 2 +// From the SH-2 interface that BNO08x use. enum class PacketErrorCode : uint8_t { - SENSOR_TIMEOUT = 1, + NOT_APPLICABLE = 0, + POWER_ON_RESET = 1, + INTERNAL_SYSTEM_RESET = 2, + WATCHDOG_TIMEOUT = 3, + EXTERNAL_RESET = 4, + OTHER = 5, }; #endif // SLIMEVR_PACKETS_H_ diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index c4e466a4..4466b513 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -331,7 +331,7 @@ void ICM20948Sensor::checkSensorTimeout() { ); networkConnection.sendSensorError( this->sensorId, - static_cast(PacketErrorCode::SENSOR_TIMEOUT) + static_cast(PacketErrorCode::WATCHDOG_TIMEOUT) ); lastData = currenttime; } diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index a38b259d..ca563db4 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -215,7 +215,7 @@ class SoftFusionSensor : public Sensor { ); networkConnection.sendSensorError( this->sensorId, - static_cast(PacketErrorCode::SENSOR_TIMEOUT) + static_cast(PacketErrorCode::WATCHDOG_TIMEOUT) ); }