From 5f928d7c92f8b1314c24f38c2b63068ceb51057e Mon Sep 17 00:00:00 2001
From: Nik Rozman <admin@piskot.si>
Date: Thu, 19 Mar 2026 20:19:26 +0100
Subject: [PATCH] Don't calibrate while device is moving

---
 source/sleep.cpp | 53 +++++++++++++++++++++++++++++++++++++++++-------
 1 file changed, 46 insertions(+), 7 deletions(-)

diff --git a/source/sleep.cpp b/source/sleep.cpp
index 9e26e3a..a0dbd05 100644
--- a/source/sleep.cpp
+++ b/source/sleep.cpp
@@ -34,6 +34,17 @@ static volatile bool pendingWakeRecal  = false;  // set only when recal is also
 // has been off long enough for thermal drift to matter (~5 minutes).
 static constexpr unsigned long RECAL_AFTER_LP_MS = 5UL * 60UL * 1000UL;
 
+// Post-wake stillness gate: don't calibrate while the device is moving.
+// Each axis of the gyro must stay below RECAL_STILL_DPS for RECAL_STILL_FRAMES
+// consecutive loop ticks before calibration fires. If the device keeps moving
+// past RECAL_WAIT_MAX_MS, recal is skipped and the pre-sleep bias is kept.
+static constexpr float         RECAL_STILL_DPS    = 10.0f;  // deg/s per axis
+static constexpr uint8_t       RECAL_STILL_FRAMES = 10;     // ~100 ms at 10 ms/frame
+static constexpr unsigned long RECAL_WAIT_MAX_MS  = 3000;   // give up after 3 s
+
+static uint8_t       recalStillFrames = 0;
+static unsigned long recalLastWarnMs  = 0;
+
 // I2C helpers - Wire1 at 0x6A (SA0 LOW on XIAO nRF52840 Sense) 
 static uint8_t lsmRead(uint8_t reg) {
   Wire1.beginTransmission(LSM_ADDR);
@@ -238,15 +249,43 @@ bool sleepManagerUpdate(unsigned long nowMs, bool idle, bool bleConnected) {
     }
   }
 
-  // Gyro settling after wake 
+  // Post-wake recalibration — gated on device being still
   if (pendingWakeRecal) {
-    if (nowMs - wakeSettleMs >= 120) {
-      pendingWakeRecal = false;
-      wakeSettleMs     = 0;
-      extern void calibrateGyroBias();
-      calibrateGyroBias();
-      Serial.println("[SLEEP] Post-wake recal done");
+    if (nowMs - wakeSettleMs < 120) return true;  // initial hardware settle
+
+    // Sample gyro magnitude; each axis must be below threshold
+    float gx = fabsf(imu.readFloatGyroX());
+    float gy = fabsf(imu.readFloatGyroY());
+    float gz = fabsf(imu.readFloatGyroZ());
+    bool still = (gx < RECAL_STILL_DPS && gy < RECAL_STILL_DPS && gz < RECAL_STILL_DPS);
+
+    if (!still) {
+      recalStillFrames = 0;
+      // Rate-limited log so we don't flood serial while waiting
+      if (nowMs - recalLastWarnMs >= 500) {
+        recalLastWarnMs = nowMs;
+        Serial.print("[SLEEP] Waiting for still: gx="); Serial.print(gx, 1);
+        Serial.print(" gy="); Serial.print(gy, 1);
+        Serial.print(" gz="); Serial.println(gz, 1);
+      }
+      if (nowMs - wakeSettleMs >= RECAL_WAIT_MAX_MS) {
+        // Device never settled — keep pre-sleep bias rather than corrupt it
+        pendingWakeRecal = false;
+        recalStillFrames = 0;
+        recalLastWarnMs  = 0;
+        Serial.println("[SLEEP] Recal skipped — device still moving after timeout");
+      }
+      return true;
     }
+
+    // Device is still — accumulate consecutive still frames
+    if (++recalStillFrames < RECAL_STILL_FRAMES) return true;
+
+    pendingWakeRecal = false;
+    recalStillFrames = 0;
+    recalLastWarnMs  = 0;
+    calibrateGyroBias();
+    Serial.println("[SLEEP] Post-wake recal done");
     return true;
   }