Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add IMU timeout detection to SoftFusionSensor #376

Open
wants to merge 8 commits into
base: main
Choose a base branch
from
Open

Add IMU timeout detection to SoftFusionSensor #376

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
bd987c3
Add IMU timeout detection to `SoftFusionSensor`
pembem22 Jan 13, 2025
2c01aad
Early return, use constexpr
pembem22 Jan 13, 2025
1de877b
Define a sensor timeout error code
pembem22 Jan 14, 2025
ebf4366
Use enum instead of define
pembem22 Jan 16, 2025
f4c2835
Avoid false timeout on timer overflow
pembem22 Jan 19, 2025
6a535a6
Revert "Avoid false timeout on timer overflow"
pembem22 Jan 19, 2025
a1b55a5
Use millis instead of micros
pembem22 Jan 19, 2025
1c2680e
Use SH-2 error codes
pembem22 Jan 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions src/network/packets.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,4 +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 {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That seems not to be defined till now. On BNO it sends the following.
https://github.com/SlimeVR/SlimeVR-Tracker-ESP/blob/main/src/sensors/bno080sensor.cpp#L220-L223
//Get the reason for the last reset
//1 = POR, 2 = Internal reset, 3 = Watchdog, 4 = External reset, 5 = Other

NOT_APPLICABLE = 0,
POWER_ON_RESET = 1,
INTERNAL_SYSTEM_RESET = 2,
WATCHDOG_TIMEOUT = 3,
EXTERNAL_RESET = 4,
OTHER = 5,
};

#endif // SLIMEVR_PACKETS_H_
5 changes: 4 additions & 1 deletion src/sensors/icm20948sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -329,7 +329,10 @@ void ICM20948Sensor::checkSensorTimeout() {
addr,
currenttime - lastData
);
networkConnection.sendSensorError(this->sensorId, 1);
networkConnection.sendSensorError(
this->sensorId,
static_cast<uint8_t>(PacketErrorCode::WATCHDOG_TIMEOUT)
);
lastData = currenttime;
}
}
Expand Down
23 changes: 23 additions & 0 deletions src/sensors/softfusion/softfusionsensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -199,6 +199,26 @@ class SoftFusionSensor : public Sensor {
, m_sensor(I2CImpl(i2cAddress), m_Logger) {}
~SoftFusionSensor() {}

void checkSensorTimeout() {
uint32_t now = millis();
constexpr uint32_t sensorTimeoutMillis = 2e3; // 2 seconds
if (m_lastRotationUpdateMillis + sensorTimeoutMillis > now) {
return;
}

working = false;
m_status = SensorStatus::SENSOR_ERROR;
m_Logger.error(
"Sensor timeout I2C Address 0x%02x delaytime: %d ms",
addr,
now - m_lastRotationUpdateMillis
);
networkConnection.sendSensorError(
this->sensorId,
static_cast<uint8_t>(PacketErrorCode::WATCHDOG_TIMEOUT)
);
}

void motionLoop() override final {
sendTempIfNeeded();

Expand All @@ -218,9 +238,11 @@ class SoftFusionSensor : public Sensor {
);
optimistic_yield(100);
if (!m_fusion.isUpdated()) {
checkSensorTimeout();
return;
}
hadData = true;
m_lastRotationUpdateMillis = millis();
m_fusion.clearUpdated();
}

Expand Down Expand Up @@ -615,6 +637,7 @@ class SoftFusionSensor : public Sensor {

SensorStatus m_status = SensorStatus::SENSOR_OFFLINE;
uint32_t m_lastPollTime = micros();
uint32_t m_lastRotationUpdateMillis = 0;
uint32_t m_lastRotationPacketSent = 0;
uint32_t m_lastTemperaturePacketSent = 0;
};
Expand Down