Simplify tracking
This commit is contained in:
@@ -186,24 +186,11 @@ void sleepManagerWakeIMU() {
|
||||
lpEnteredMs = 0;
|
||||
|
||||
// Reset motion filter state to prevent a cursor jump on the first frame.
|
||||
// After sleep: angleX/Y are stale, gravX/Y/Z drifted, accumX/Y is dirty,
|
||||
// and lastTime is old so dt would be huge on the first loop iteration.
|
||||
// Zeroing these here means the first frame integrates 0 motion cleanly.
|
||||
extern float angleX, angleY;
|
||||
extern float accumX, accumY;
|
||||
extern float gravX, gravY, gravZ;
|
||||
extern float prevAx, prevAy, prevAz;
|
||||
extern unsigned long lastTime;
|
||||
angleX = angleY = 0.0f;
|
||||
accumX = accumY = 0.0f;
|
||||
// Reseed gravity from current accel so projection is correct immediately.
|
||||
// Can't call imu.readFloat* here (gyro not fully settled), but accel is
|
||||
// already running - read it directly via Wire1.
|
||||
// Simpler: just reset to neutral [0,0,1] and let the LP filter converge
|
||||
// over the first ~20 frames (200 ms) of real use.
|
||||
gravX = 0.0f; gravY = 0.0f; gravZ = 1.0f;
|
||||
prevAx = 0.0f; prevAy = 0.0f; prevAz = 0.0f;
|
||||
// Set lastTime to now so the first dt = 0 rather than (now - sleepEntryTime)
|
||||
lastTime = millis();
|
||||
|
||||
sleepStage = SLEEP_AWAKE;
|
||||
|
||||
Reference in New Issue
Block a user