Add motion tracking and calibration features

source/sleep.cpp

@@ -1,5 +1,6 @@
 #include "sleep.h"
 #include "imu.h"       // config.h REG_* macros
+#include "motion.h"
 #include <bluefruit.h> // sd_app_evt_wait()
 #include "Wire.h"
 
@@ -204,11 +205,7 @@ void sleepManagerWakeIMU() {
   idleEnteredMs = 0;
   lpEnteredMs   = 0;
 
-  // Reset motion filter state to prevent a cursor jump on the first frame.
-  extern float accumX, accumY;
-  extern unsigned long lastTime;
-  accumX = accumY = 0.0f;
-  lastTime = millis();
+  lastMotionMs = millis();
 
   sleepStage = SLEEP_AWAKE;
   if (needsRecal)
@@ -262,9 +259,18 @@ bool sleepManagerUpdate(unsigned long nowMs, bool idle, bool bleConnected) {
     }
   }
 
+  // Every wake: skip the gyro turn-on transient (measured: ~30 ms of samples up to
+  // ~265 dps off), then restart tracking from a clean FIFO so it can't jump the cursor.
+  if (pendingWakeSettle) {
+    if ((long)(nowMs - wakeSettleMs) < 120) return true;  // signed: wakeSettleMs can be newer than nowMs
+    pendingWakeSettle = false;
+    imuFifoReset();
+    motionReset();  // gravity is re-seeded from the first fresh accel sample
+  }
+
   // Post-wake recalibration — gated on device being still
   if (pendingWakeRecal) {
-    if (nowMs - wakeSettleMs < 120) return true;  // initial hardware settle
+    if ((long)(nowMs - wakeSettleMs) < 120) return true;  // initial hardware settle
 
     // Sample gyro magnitude; each axis must be below threshold
     float gx = fabsf(imu.readFloatGyroX());
@@ -281,7 +287,7 @@ bool sleepManagerUpdate(unsigned long nowMs, bool idle, bool bleConnected) {
         Serial.print(" gy="); Serial.print(gy, 1);
         Serial.print(" gz="); Serial.println(gz, 1);
       }
-      if (nowMs - wakeSettleMs >= RECAL_WAIT_MAX_MS) {
+      if ((long)(nowMs - wakeSettleMs) >= (long)RECAL_WAIT_MAX_MS) {
         // Device never settled — keep pre-sleep bias rather than corrupt it
         pendingWakeRecal = false;
         recalStillFrames = 0;