Add motion tracking and calibration features

source/buttons.cpp

@@ -9,6 +9,7 @@ extern Config cfg;
 static uint8_t physBtnMask = 0;
 static uint8_t rawMaskPrev = 0;
 static unsigned long debounceMs = 0;
+unsigned long btnLastEdgeMs = 0;
 static const unsigned long DEBOUNCE_MS = 20;
 
 // Double-press detection for left button
@@ -61,7 +62,7 @@ void processPhysicalButtons() {
   if (BTN_RIGHT_PIN  != BTN_PIN_NONE && digitalRead(BTN_RIGHT_PIN)  == LOW) rawMask |= MOUSE_BUTTON_RIGHT;
   if (BTN_MIDDLE_PIN != BTN_PIN_NONE && digitalRead(BTN_MIDDLE_PIN) == LOW) rawMask |= MOUSE_BUTTON_MIDDLE;
 
-  if (rawMask != rawMaskPrev) { rawMaskPrev = rawMask; debounceMs = now; }
+  if (rawMask != rawMaskPrev) { rawMaskPrev = rawMask; debounceMs = now; btnLastEdgeMs = now; }
   if (rawMask != physBtnMask && (now - debounceMs >= DEBOUNCE_MS)) {
     uint8_t newMask = rawMask;
     uint8_t pressed  = newMask & ~physBtnMask;

source/buttons.h

@@ -4,4 +4,5 @@
 #ifdef FEATURE_PHYSICAL_BUTTONS
 void setupPhysicalButtons();
 void processPhysicalButtons();
+extern unsigned long btnLastEdgeMs;  // millis() of the last raw button edge (0 = none)
 #endif

source/config.h

@@ -7,7 +7,7 @@
 #define FEATURE_IMU_STREAM
 // #define FEATURE_TAP_DETECTION
 #define FEATURE_TEMP_COMPENSATION
-#define FEATURE_AUTO_RECAL
+#define FEATURE_AUTO_RECAL          // continuous gyro bias tracking while at rest
 #define FEATURE_BATTERY_MONITOR
 #define FEATURE_BOOT_LOOP_DETECT
 #define FEATURE_PHYSICAL_BUTTONS
@@ -55,7 +55,7 @@
 
 // Persistence 
 #define CONFIG_FILENAME  "/imu_mouse_cfg.bin"
-#define CONFIG_MAGIC     0xDEAD123EUL
+#define CONFIG_MAGIC     0xDEAD123FUL
 
 // Physical button pin assignments (hardcoded - set to 0xFF to disable a button)
 // Valid pin numbers: 0-10 (Arduino D0-D10 on XIAO nRF52840 Sense)
@@ -69,7 +69,7 @@
 // via the web UI and persisted in flash.  Bits not listed here are reserved = 0.
 #define FLAG_TAP_ENABLED        0x01  // Tap detection active (requires restart)
 #define FLAG_TEMP_COMP_ENABLED  0x02  // Temperature gyro-drift compensation
-#define FLAG_AUTO_RECAL_ENABLED 0x04  // Auto-recalibrate after long idle
+#define FLAG_AUTO_RECAL_ENABLED 0x04  // Continuously re-estimate gyro bias whenever the remote is at rest
 #define FLAG_ALL_DEFAULT        (FLAG_TAP_ENABLED | FLAG_TEMP_COMP_ENABLED | FLAG_AUTO_RECAL_ENABLED)
 
 // Enums 
@@ -91,12 +91,12 @@ enum TapAction : uint8_t {
 struct Config {
   uint32_t   magic;
   float      sensitivity;
-  float      deadZone;
+  float      deadZone;         // soft dead zone radius on smoothed angular speed, rad/s
   float      accelStrength;
   CurveType  curve;
   uint8_t    axisFlip;
   ChargeMode chargeMode;
-  uint8_t    tapThreshold; // 1–31 → REG_TAP_THS_6D bits[4:0]; 1 LSB = 62.5 mg at ±2g
+  uint8_t    tapThreshold; // 1–31 → REG_TAP_THS_6D bits[4:0]; 1 LSB = 125 mg at ±4g
   TapAction  tapAction;    // what a double-tap does
   uint8_t    tapKey;       // HID keycode (used when tapAction == TAP_ACTION_KEY)
   uint8_t    tapMod;       // HID modifier byte (used when tapAction == TAP_ACTION_KEY)
@@ -145,12 +145,12 @@ extern TelemetryPacket telem;
 // ImuPacket (14 bytes) 
 #ifdef FEATURE_IMU_STREAM
 struct __attribute__((packed)) ImuPacket {
-  int16_t  gyroX_mDPS;  // [0]  pitch axis (nod up/down → cursor Y)
-  int16_t  gyroZ_mDPS;  // [2]  yaw axis   (pan left/right → cursor X)
+  int16_t  gyroX_mDPS;  // [0]  pitch rate, roll-compensated (nod up/down → cursor Y)
+  int16_t  gyroZ_mDPS;  // [2]  yaw rate, roll-compensated   (pan left/right → cursor X)
   int16_t  accelX_mg;   // [4]
   int16_t  accelY_mg;   // [6]
   int16_t  accelZ_mg;   // [8]
-  int8_t   moveX;       // [10]
+  int8_t   moveX;       // [10] cursor counts sent since the previous packet
   int8_t   moveY;       // [11]
   uint8_t  flags;       // [12]  bit0=idle bit1=singleTap bit2=doubleTap
   uint8_t  _pad;        // [13]
@@ -159,9 +159,9 @@ static_assert(sizeof(ImuPacket) == 14, "ImuPacket must be 14 bytes");
 #endif
 
 // Tuning constants
-extern const int   LOOP_RATE_MS;
+extern const int   IMU_POLL_MS;
 extern const int   BIAS_SAMPLES;
-extern const int   IDLE_FRAMES;
+extern const unsigned long IDLE_MS;
 extern const unsigned long BATT_REPORT_MS;
 extern const unsigned long TELEMETRY_MS;
 extern const unsigned long HEARTBEAT_MS;
@@ -179,13 +179,8 @@ extern const float BATT_CRITICAL;
 #ifdef FEATURE_TEMP_COMPENSATION
   extern const float TEMP_COMP_COEFF_DPS_C;
 #endif
-#ifdef FEATURE_AUTO_RECAL
-  extern const unsigned long AUTO_RECAL_MS;
-#endif
 
 // Global state
-extern float accumX, accumY;
-extern float biasGX, biasGY, biasGZ;
 extern float calTempC;
 extern float cachedTempC;
 
@@ -207,9 +202,7 @@ extern bool pendingReset;
   extern bool pendingOTA;
 #endif
 extern ChargeStatus lastChargeStatus;
-extern int           idleFrames;
-extern unsigned long idleStartMs;
-extern unsigned long lastTime;
+extern unsigned long lastMotionMs;
 extern unsigned long lastBattTime;
 extern unsigned long lastHeartbeat;
 extern unsigned long lastTelemetry;

source/imu.cpp

@@ -1,10 +1,28 @@
 #include "imu.h"
+#include "motion.h"
 #include "Wire.h"
 #include <math.h>
 
 LSM6DS3 imu(I2C_MODE, 0x6A);
 
-// I2C helpers 
+uint32_t imuFifoOverruns = 0;
+
+// Registers not in config.h
+#define REG_FIFO_CTRL3   0x08
+#define REG_FIFO_CTRL5   0x0A
+#define REG_CTRL2_G      0x11
+#define REG_CTRL3_C      0x12
+#define REG_CTRL4_C      0x13
+#define REG_FIFO_STATUS1 0x3A
+#define REG_FIFO_DATA_L  0x3E
+
+#define FIFO_CTRL5_CONT_208HZ 0x2E  // ODR_FIFO=208 Hz, FIFO_MODE=continuous
+
+static const int CAL_TIMEOUT_MS = 3000;
+// Gyro peak-to-peak (any axis) above this during calibration means the device moved.
+static const float CAL_STILL_PP_DPS = 1.0f;
+
+// I2C helpers
 void imuWriteReg(uint8_t reg, uint8_t val) {
   // LSM6DS3 is on Wire1 (internal I2C, SDA=P0.17, SCL=P0.16), NOT Wire (external pins 4/5)
   Wire1.beginTransmission(0x6A); Wire1.write(reg); Wire1.write(val); Wire1.endTransmission();
@@ -16,40 +34,168 @@ uint8_t imuReadReg(uint8_t reg) {
   return Wire1.available() ? Wire1.read() : 0;
 }
 
-// Temperature 
+// Burst read using Wire1's TWIM directly, with the hardware generating the repeated
+// start and the STOP (LASTTX->STARTRX, LASTRX->STOP shortcuts).
+// Wire1.requestFrom() triggers STOP from software after the LASTRX event; when a
+// SoftDevice interrupt delays that, the TWIM clocks in extra bytes. Here that is
+// not harmless: an over-read of FIFO_STATUS runs into FIFO_DATA_OUT and silently
+// consumes FIFO words, which shows up as corrupted, shifted samples.
+uint32_t imuI2cErrors = 0;
+
+static bool imuReadBlock(uint8_t reg, uint8_t* buf, uint16_t len) {
+  NRF_TWIM_Type* t = NRF_TWIM1;
+  static uint8_t txReg;
+  txReg = reg;
+  t->EVENTS_STOPPED = 0; t->EVENTS_ERROR = 0; t->EVENTS_SUSPENDED = 0;
+  t->EVENTS_LASTTX = 0;  t->EVENTS_LASTRX = 0;
+  t->EVENTS_TXSTARTED = 0; t->EVENTS_RXSTARTED = 0;
+  t->ERRORSRC = t->ERRORSRC;
+  t->ADDRESS = 0x6A;
+  t->TXD.PTR = (uint32_t)&txReg; t->TXD.MAXCNT = 1;
+  t->RXD.PTR = (uint32_t)buf;    t->RXD.MAXCNT = len;
+  t->SHORTS = TWIM_SHORTS_LASTTX_STARTRX_Msk | TWIM_SHORTS_LASTRX_STOP_Msk;
+  t->TASKS_RESUME = 1;
+  t->TASKS_STARTTX = 1;
+  uint32_t t0 = micros();
+  bool ok = true;
+  while (!t->EVENTS_STOPPED) {
+    if (t->EVENTS_ERROR || micros() - t0 > 20000) {
+      ok = false;
+      t->TASKS_STOP = 1;
+      uint32_t t1 = micros();
+      while (!t->EVENTS_STOPPED && micros() - t1 < 2000) {}
+      break;
+    }
+  }
+  t->SHORTS = 0;
+  if (t->RXD.AMOUNT != len) ok = false;
+  t->EVENTS_STOPPED = 0; t->EVENTS_ERROR = 0;
+  t->EVENTS_LASTTX = 0;  t->EVENTS_LASTRX = 0;
+  t->EVENTS_TXSTARTED = 0; t->EVENTS_RXSTARTED = 0;
+  t->ERRORSRC = t->ERRORSRC;
+  if (!ok) imuI2cErrors++;
+  return ok;
+}
+
+// Configuration
+// Called after imu.begin(). The library settings are kept in sync in main.cpp so
+// its readFloat*() helpers (used by the sleep manager) still scale correctly.
+void imuConfigure() {
+  Wire1.setClock(400000);
+  imuWriteReg(REG_CTRL3_C,  0x44);  // BDU=1, IF_INC=1
+  imuWriteReg(REG_CTRL4_C,  0x00);  // clear bit7 set by the library (DEN_XL_EN on the TR-C)
+  imuWriteReg(REG_CTRL1_XL, 0x58);  // accel 208 Hz, ±4 g
+  imuWriteReg(REG_CTRL2_G,  0x58);  // gyro  208 Hz, ±1000 dps
+  imuWriteReg(REG_FIFO_CTRL3, 0x09);  // gyro + accel into FIFO, no decimation
+  delay(150);  // gyro turn-on / reconfiguration transient (first samples read 100+ dps off)
+  imuFifoReset();
+}
+
+void imuFifoReset() {
+  imuWriteReg(REG_FIFO_CTRL5, 0x00);  // bypass mode empties the FIFO
+  imuWriteReg(REG_FIFO_CTRL5, FIFO_CTRL5_CONT_208HZ);
+}
+
+// Drain up to maxSamples complete gyro+accel sets from the FIFO.
+// Each set is 6 words in the order Gx Gy Gz Ax Ay Az. FIFO_PATTERN gives the
+// index of the next word, so a partial set (after an overrun) is skipped first.
+int imuReadFifo(ImuRaw* out, int maxSamples) {
+  uint8_t st[4];
+  if (!imuReadBlock(REG_FIFO_STATUS1, st, 4)) return 0;
+  uint16_t words   = st[0] | ((uint16_t)(st[1] & 0x07) << 8);
+  uint16_t pattern = st[2] | ((uint16_t)(st[3] & 0x03) << 8);
+  if (st[1] & 0x40) imuFifoOverruns++;
+
+  while (pattern != 0 && words > 0) {
+    uint8_t skip[2];
+    if (!imuReadBlock(REG_FIFO_DATA_L, skip, 2)) return 0;
+    words--; pattern = (pattern + 1) % 6;
+  }
+
+  int n = words / 6;
+  if (n > maxSamples) n = maxSamples;
+  const int PER_READ = 16;
+  int done = 0;
+  while (done < n) {
+    int chunk = min(PER_READ, n - done);
+    uint8_t buf[PER_READ * 12];
+    // Multi-byte reads of FIFO_DATA_OUT roll back from _H to _L, so a burst
+    // returns consecutive FIFO words.
+    if (!imuReadBlock(REG_FIFO_DATA_L, buf, chunk * 12)) break;
+    for (int i = 0; i < chunk; i++) {
+      const uint8_t* b = buf + i * 12;
+      ImuRaw& s = out[done + i];
+      s.gx = (int16_t)(b[0]  | (b[1]  << 8));
+      s.gy = (int16_t)(b[2]  | (b[3]  << 8));
+      s.gz = (int16_t)(b[4]  | (b[5]  << 8));
+      s.ax = (int16_t)(b[6]  | (b[7]  << 8));
+      s.ay = (int16_t)(b[8]  | (b[9]  << 8));
+      s.az = (int16_t)(b[10] | (b[11] << 8));
+    }
+    done += chunk;
+  }
+  return done;
+}
+
+// Temperature
 float readIMUTemp() {
   int16_t raw = (int16_t)((imuReadReg(REG_OUT_TEMP_H) << 8) | imuReadReg(REG_OUT_TEMP_L));
   return 25.0f + (float)raw / 256.0f;
 }
 
-// Calibration 
+// Calibration
+// Averages ~1 s of FIFO samples. If the device moved, the result is only used
+// when there is no previous bias; the continuous bias tracker refines it later.
 void calibrateGyroBias() {
   Serial.println("[CAL] Hold still...");
-  double sx=0, sy=0, sz=0;
-  for (int i=0; i<BIAS_SAMPLES; i++) {
-    sx += imu.readFloatGyroX(); sy += imu.readFloatGyroY(); sz += imu.readFloatGyroZ();
-    digitalWrite(LED_GREEN, (i%20 < 10)); delay(5);  // green flutter during calibration
+  imuFifoReset();
+  double sum[6] = {0};
+  int16_t mn[3] = {32767, 32767, 32767}, mx[3] = {-32768, -32768, -32768};
+  int n = 0;
+  unsigned long t0 = millis();
+  ImuRaw buf[16];
+  while (n < BIAS_SAMPLES && millis() - t0 < (unsigned long)CAL_TIMEOUT_MS) {
+    int k = imuReadFifo(buf, 16);
+    for (int i = 0; i < k; i++) {
+      const int16_t v[6] = {buf[i].gx, buf[i].gy, buf[i].gz, buf[i].ax, buf[i].ay, buf[i].az};
+      for (int j = 0; j < 6; j++) sum[j] += v[j];
+      for (int j = 0; j < 3; j++) { mn[j] = min(mn[j], v[j]); mx[j] = max(mx[j], v[j]); }
+    }
+    n += k;
+    digitalWrite(LED_GREEN, ((millis() - t0) / 100) % 2);  // green flutter during calibration
+    delay(5);
+  }
+  digitalWrite(LED_GREEN, HIGH);
+
+  if (n == 0) { Serial.println("[CAL] No IMU samples - calibration skipped"); return; }
+
+  float pp = 0;
+  for (int j = 0; j < 3; j++) pp = max(pp, (mx[j] - mn[j]) * GYRO_DPS_PER_LSB);
+  bool still = pp < CAL_STILL_PP_DPS;
+  float bias[3], accel[3];
+  for (int j = 0; j < 3; j++) {
+    bias[j]  = (float)(sum[j] / n) * GYRO_RAD_PER_LSB;
+    accel[j] = (float)(sum[j + 3] / n) * ACCEL_G_PER_LSB;
   }
-  biasGX = (float)(sx/BIAS_SAMPLES);
-  biasGY = (float)(sy/BIAS_SAMPLES);
-  biasGZ = (float)(sz/BIAS_SAMPLES);
   calTempC = readIMUTemp();
-  accumX = accumY = 0.0f;
+  bool applied = motionApplyCalibration(bias, accel, still);
 
   #ifdef FEATURE_TELEMETRY
     statRecalCount++;
-    float bxr = biasGX*(PI/180.f), byr = biasGY*(PI/180.f), bzr = biasGZ*(PI/180.f);
-    statBiasRms = sqrtf((bxr*bxr + byr*byr + bzr*bzr) / 3.0f);
   #endif
 
-  digitalWrite(LED_GREEN, HIGH);  // off after calibration
-  Serial.print("[CAL] T="); Serial.print(calTempC,1);
-  Serial.print("C  bias="); Serial.print(biasGX,4);
-  Serial.print(","); Serial.print(biasGY,4);
-  Serial.print(","); Serial.println(biasGZ,4);
+  Serial.print("[CAL] T="); Serial.print(calTempC, 1);
+  Serial.print("C n="); Serial.print(n);
+  Serial.print(" p-p="); Serial.print(pp, 2); Serial.print("dps");
+  Serial.print(" bias(dps)="); Serial.print(bias[0] * (180.f / PI), 3);
+  Serial.print(","); Serial.print(bias[1] * (180.f / PI), 3);
+  Serial.print(","); Serial.print(bias[2] * (180.f / PI), 3);
+  Serial.println(applied ? (still ? " applied" : " applied (moved - will refine when still)")
+                         : " ignored (device moved, keeping previous bias)");
+  imuFifoReset();
 }
 
-// Motion curve 
+// Motion curve
 float applyCurve(float v) {
   switch (cfg.curve) {
     case CURVE_SQUARE: return (v >= 0 ? 1.f : -1.f) * v * v;

source/imu.h

@@ -4,8 +4,27 @@
 
 extern LSM6DS3 imu;
 
+// Sensor configuration used by the tracking pipeline.
+// Gyro ±1000 dps (35 mdps/LSB): fine enough that quantisation sits below the
+// sensor noise floor, while hand flicks never saturate. Accel ±4 g.
+// Every sample is read from the hardware FIFO, so integration uses the sensor's
+// own sample clock instead of loop timing, and nothing is lost when the loop stalls.
+constexpr float IMU_ODR_HZ        = 208.0f;
+constexpr float IMU_DT            = 1.0f / IMU_ODR_HZ;
+constexpr float GYRO_DPS_PER_LSB  = 0.035f;
+constexpr float GYRO_RAD_PER_LSB  = GYRO_DPS_PER_LSB * (PI / 180.0f);
+constexpr float ACCEL_G_PER_LSB   = 0.000122f;
+
+struct ImuRaw { int16_t gx, gy, gz, ax, ay, az; };
+
+extern uint32_t imuFifoOverruns;
+extern uint32_t imuI2cErrors;
+
 void imuWriteReg(uint8_t reg, uint8_t val);
 uint8_t imuReadReg(uint8_t reg);
+void imuConfigure();
+void imuFifoReset();
+int  imuReadFifo(ImuRaw* out, int maxSamples);
 float readIMUTemp();
 void calibrateGyroBias();
 float applyCurve(float v);

source/main.cpp

@@ -15,6 +15,7 @@
 
 #include "config.h"
 #include "imu.h"
+#include "motion.h"
 #include "ble_config.h"
 #include "battery.h"
 #include "buttons.h"
@@ -44,7 +45,7 @@ File cfgFile(InternalFS);
 // Config definitions 
 Config cfg;
 const Config CFG_DEFAULTS = {
-  CONFIG_MAGIC, 600.0f, 0.060f, 0.08f, CURVE_LINEAR, 0x00, CHARGE_SLOW,
+  CONFIG_MAGIC, 600.0f, 0.020f, 0.08f, CURVE_LINEAR, 0x00, CHARGE_SLOW,
   /*tapThreshold=*/12, /*tapAction=*/TAP_ACTION_KEY, /*tapKey=*/0x04, /*tapMod=*/0x03,  // Ctrl+Shift+A
   /*jerkThreshold=*/2000.0f, /*tapFreezeEnabled=*/1, /*featureFlags=*/FLAG_ALL_DEFAULT
 };
@@ -55,10 +56,16 @@ TelemetryPacket telem = {};
 #endif
 
 // Tuning constants
-const int   LOOP_RATE_MS       = 10;
-const float SMOOTH_ALPHA       = 0.65f;  // single-pole low-pass for cursor smoothing
+const int   IMU_POLL_MS        = 2;     // FIFO poll period; samples arrive at 208 Hz
 const int   BIAS_SAMPLES       = 200;
-const int   IDLE_FRAMES        = 150;
+const unsigned long IDLE_MS    = 1500;  // no motion for this long = idle (sleep countdown starts)
+// Requested BLE connection interval (units of 1.25 ms). HID reports are paced to the
+// negotiated interval, so the notify queue never fills and the loop never blocks.
+// Slave latency lets the radio skip empty events while the cursor is still; reports
+// are still sent at the next event, so it adds no pointer latency.
+const uint16_t CONN_INTERVAL_MIN = 6;   // 7.5 ms
+const uint16_t CONN_INTERVAL_MAX = 9;   // 11.25 ms
+const uint16_t CONN_SLAVE_LATENCY = 4;
 const unsigned long BATT_REPORT_MS       = 20000;
 const unsigned long TELEMETRY_MS         = 1000;
 const unsigned long HEARTBEAT_MS         = 10000;
@@ -76,13 +83,8 @@ const float BATT_CRITICAL                = 3.10f;
 #ifdef FEATURE_TEMP_COMPENSATION
   const float TEMP_COMP_COEFF_DPS_C = 0.004f;
 #endif
-#ifdef FEATURE_AUTO_RECAL
-  const unsigned long AUTO_RECAL_MS = 5UL * 60UL * 1000UL;
-#endif
 
 // Global state definitions
-float accumX = 0, accumY = 0;
-float biasGX = 0, biasGY = 0, biasGZ = 0;
 float calTempC    = 25.0f;
 float cachedTempC = 25.0f;
 
@@ -108,7 +110,6 @@ float cachedTempC = 25.0f;
   const unsigned long  STREAM_BACKOFF_MS     = 500;  // cooldown window
 #endif
 
-uint32_t loopStalls = 0;          // loop iterations where dt > 20ms (behind schedule)
 bool pendingCal   = false;
 bool pendingReset = false;
 #ifdef FEATURE_OTA
@@ -118,9 +119,8 @@ bool pendingReset = false;
 
 ChargeStatus lastChargeStatus = CHGSTAT_DISCHARGING;
 
-int           idleFrames  = 0;
-unsigned long idleStartMs = 0;
-unsigned long lastTime      = 0;
+unsigned long lastMotionMs  = 0;
+unsigned long lastImuPoll   = 0;
 unsigned long lastBattTime  = 0;
 unsigned long lastHeartbeat = 0;
 unsigned long lastTelemetry = 0;
@@ -137,6 +137,15 @@ unsigned long bootStartMs   = 0;
 bool safeMode         = false;
 bool bootCountCleared = false;
 
+// Connection 
+static volatile unsigned long connectedAtMs = 0;
+static volatile bool          connParamPending = false;
+static void onConnect(uint16_t connHdl) {
+  (void)connHdl;
+  connectedAtMs = millis();
+  connParamPending = true;
+}
+
 // Advertising 
 static void startAdvertising() {
   Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
@@ -190,13 +199,22 @@ void setup() {
   Bluefruit.begin(1, 0);
   Bluefruit.setTxPower(4);
   Bluefruit.setName(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
-  Bluefruit.Periph.setConnInterval(16, 32);  // 20-40ms - wider interval reduces SoftDevice TX stalls
+  Bluefruit.Periph.setConnInterval(CONN_INTERVAL_MIN, CONN_INTERVAL_MAX);
+  Bluefruit.Periph.setConnSlaveLatency(CONN_SLAVE_LATENCY);
+  Bluefruit.Periph.setConnectCallback(onConnect);
 
   Wire1.begin();  // LSM6DS3 is on internal I2C bus (Wire1), must init before imu.begin()
+  // Keep the library's scaling in sync with imuConfigure() (sleep.cpp uses readFloatGyro*)
+  imu.settings.gyroRange       = 1000;
+  imu.settings.gyroSampleRate  = 208;
+  imu.settings.accelRange      = 4;
+  imu.settings.accelSampleRate = 208;
+  imu.settings.accelODROff     = 0;
   if (imu.begin() != 0) {
     Serial.println("[ERROR] IMU init failed");
     while(1) { digitalWrite(LED_RED, !digitalRead(LED_RED)); delay(100); }  // fault: red rapid blink
   }
+  imuConfigure();
   Serial.println("[OK] IMU ready");
 
   #ifdef FEATURE_TAP_DETECTION
@@ -270,7 +288,44 @@ void setup() {
   Serial.println();
 
   bootStartMs = millis();
-  lastTime = lastBattTime = lastHeartbeat = lastTelemetry = millis();
+  lastMotionMs = lastImuPoll = lastBattTime = lastHeartbeat = lastTelemetry = millis();
+}
+
+
+// Debug state (serial commands)
+static bool          statsEnabled = false;   // 's' - 1 Hz pipeline stats
+static bool          rawLogEnabled = false;  // 'l' - every FIFO sample as CSV
+static unsigned long diagUntil = 0;          // 'd' - 10 s of yaw/pitch/roll
+static uint32_t statHidReports = 0, statHidFails = 0, statSamples = 0;
+static int32_t  statSentX = 0, statSentY = 0;
+static uint32_t statMaxHidGapUs = 0;
+// 't' - synthetic sweep, totals reported when it ends
+static bool     testRunning = false;
+static int32_t  testPosX = 0, testNegX = 0, testPosY = 0, testNegY = 0;
+static uint32_t testReports = 0, testFails = 0;
+
+static void printStats(unsigned long now) {
+  const MotionDebug& m = motionDebug();
+  BLEConnection* c = Bluefruit.connected() ? Bluefruit.Connection(Bluefruit.connHandle()) : nullptr;
+  Serial.print("[STAT] smp/s="); Serial.print(statSamples);
+  Serial.print(" ovr="); Serial.print(imuFifoOverruns);
+  Serial.print(" i2cErr="); Serial.print(imuI2cErrors);
+  Serial.print(" hid/s="); Serial.print(statHidReports);
+  Serial.print(" fail="); Serial.print(statHidFails);
+  Serial.print(" sent="); Serial.print(statSentX); Serial.print(","); Serial.print(statSentY);
+  Serial.print(" maxGap="); Serial.print(statMaxHidGapUs / 1000.0f, 1); Serial.print("ms");
+  Serial.print(" ci="); if (c) { Serial.print(c->getConnectionInterval() * 1.25f, 2); Serial.print("ms"); } else Serial.print("-");
+  Serial.print(" bias(mdps)=");
+  for (int i = 0; i < 3; i++) { Serial.print(m.bias[i] * (180.f / PI) * 1000.f, 1); if (i < 2) Serial.print(","); }
+  Serial.print(m.biasValid ? " valid" : " UNCAL");
+  Serial.print(m.still ? " still" : " moving");
+  Serial.print(" upd="); Serial.print(m.biasUpdates);
+  Serial.print(" roll="); Serial.print(m.rollDeg, 1);
+  Serial.print(" up="); Serial.print(m.up[0], 3); Serial.print(","); Serial.print(m.up[1], 3); Serial.print(","); Serial.print(m.up[2], 3);
+  Serial.print(" T="); Serial.println(cachedTempC, 2);
+  statSamples = statHidReports = statHidFails = 0;
+  statMaxHidGapUs = 0;
+  (void)now;
 }
 
 // Loop 
@@ -285,13 +340,20 @@ void loop() {
     }
   #endif
 
-  // Serial commands: 'c' = calibrate, 'r' = factory reset, 'd' = axis diagnostic
-  static unsigned long diagUntil = 0;
+  // Serial commands: 'c' calibrate, 'r' factory reset, 'd' axis diagnostic,
+  // 's' toggle stats, 'l' toggle raw sample log, 't' synthetic sweep test
   while (Serial.available()) {
     char cmd = Serial.read();
     if (cmd == 'c') { Serial.println("[SERIAL] Calibrate"); pendingCal   = true; }
     if (cmd == 'r') { Serial.println("[SERIAL] Reset");     pendingReset = true; }
-    if (cmd == 'd') { Serial.println("[DIAG] Printing raw gyro for 10s — pan, nod, roll one at a time"); diagUntil = now + 10000; }
+    if (cmd == 'd') { Serial.println("[DIAG] yaw/pitch (rad/s, roll-compensated) + roll for 10s"); diagUntil = now + 10000; }
+    if (cmd == 's') { statsEnabled = !statsEnabled; Serial.println(statsEnabled ? "[STAT] on" : "[STAT] off"); }
+    if (cmd == 'l') { rawLogEnabled = !rawLogEnabled; Serial.println(rawLogEnabled ? "[RAW] gx,gy,gz,ax,ay,az (LSB),roll (0.1 deg)" : "[RAW] off"); }
+    if (cmd == 't' && !testRunning) {
+      Serial.println("[TEST] Synthetic sweep: right, left, down, up (1 s each)");
+      testPosX = testNegX = testPosY = testNegY = 0; testReports = testFails = 0;
+      testRunning = true; motionStartTest();
+    }
     #ifdef FEATURE_OTA
       if (cmd == 'o') { Serial.println("[SERIAL] OTA DFU"); pendingOTA = true; }
     #endif
@@ -315,11 +377,30 @@ void loop() {
     }
   #endif
 
-  // Heartbeat LED
+  // Ask the host for a short connection interval once the link has settled.
+  if (connParamPending && Bluefruit.connected() && now - connectedAtMs >= 2000) {
+    connParamPending = false;
+    uint16_t h = Bluefruit.connHandle();
+    BLEConnection* c = Bluefruit.Connection(h);
+    if (c) {
+      Serial.print("[BLE] Connection interval "); Serial.print(c->getConnectionInterval() * 1.25f, 2); Serial.println("ms");
+      if (c->getConnectionInterval() > CONN_INTERVAL_MAX) {
+        ble_gap_conn_params_t p = { CONN_INTERVAL_MIN, CONN_INTERVAL_MAX, CONN_SLAVE_LATENCY, BLE_GAP_CONN_SUPERVISION_TIMEOUT_MS / 10 };
+        uint32_t err = sd_ble_gap_conn_param_update(h, &p);
+        Serial.print("[BLE] Requested 7.5-11.25ms interval (err="); Serial.print(err); Serial.println(")");
+      }
+    }
+  }
+
+  // Heartbeat LED (non-blocking, so it can't delay HID reports)
+  static int heartbeatLed = -1;
   if (now - lastHeartbeat >= HEARTBEAT_MS) {
     lastHeartbeat = now;
-    int led = Bluefruit.connected() ? LED_BLUE : LED_GREEN;  // blue=BT connected, green=advertising
-    digitalWrite(led, LOW); delay(HEARTBEAT_DUR); digitalWrite(led, HIGH);
+    heartbeatLed = Bluefruit.connected() ? LED_BLUE : LED_GREEN;  // blue=BT connected, green=advertising
+    digitalWrite(heartbeatLed, LOW);
+  }
+  if (heartbeatLed >= 0 && now - lastHeartbeat >= (unsigned long)HEARTBEAT_DUR) {
+    digitalWrite(heartbeatLed, HIGH); heartbeatLed = -1;
   }
 
   #ifdef FEATURE_BATTERY_MONITOR
@@ -333,101 +414,104 @@ void loop() {
   #ifdef FEATURE_PHYSICAL_BUTTONS
     processPhysicalButtons();
   #endif
+
+  // Debug modes keep the device awake so it can be measured lying still.
+  bool debugAwake = statsEnabled || rawLogEnabled || testRunning;
+  if (debugAwake && sleepStage == SLEEP_IMU_LP) sleepManagerWakeIMU();
+  bool idle = !debugAwake && (now - lastMotionMs >= IDLE_MS);
   {
-    bool idle_for_sleep = (sleepStage == SLEEP_IMU_LP) ? true
-                          : (idleFrames >= IDLE_FRAMES);
+    bool idle_for_sleep = (sleepStage == SLEEP_IMU_LP) ? true : idle;
     if (sleepManagerUpdate(now, idle_for_sleep, Bluefruit.connected())) return;
   }
 
-  if (now - lastTime < (unsigned long)LOOP_RATE_MS) return;
-  float dt = (now - lastTime) / 1000.0f;
-  lastTime = now;
-  if (dt <= 0.0f || dt > 0.5f) return;
-  // Threshold 50ms: intentional heartbeat blink (30ms) won't false-trigger;
-  // real SoftDevice stalls (100ms+) and unexpected delays still get flagged.
-  if (dt > 0.050f) { loopStalls++; Serial.print("[STALL] dt="); Serial.print(dt*1000.f,1); Serial.print("ms stalls="); Serial.println(loopStalls); }
+  static ImuRaw lastSample = {};
+  if (now - lastImuPoll >= (unsigned long)IMU_POLL_MS) {
+    lastImuPoll = now;
 
-  cachedTempC = readIMUTemp();
+    static unsigned long lastTempMs = 0;
+    if (now - lastTempMs >= 1000) { lastTempMs = now; cachedTempC = readIMUTemp(); }
 
-  #ifdef FEATURE_TELEMETRY
-    if (!safeMode && (now - lastTelemetry >= TELEMETRY_MS)) {
-      lastTelemetry = now; pushTelemetry(now);
+    #ifdef FEATURE_TELEMETRY
+      if (!safeMode && (now - lastTelemetry >= TELEMETRY_MS)) {
+        lastTelemetry = now; pushTelemetry(now);
+      }
+    #endif
+
+    // Run the tracking pipeline on every sample the IMU produced since the last poll.
+    ImuRaw samples[32];
+    int n = imuReadFifo(samples, 32);
+    for (int i = 0; i < n; i++) {
+      motionProcess(samples[i], now);
+      if (rawLogEnabled) {
+        const ImuRaw& r = samples[i];
+        Serial.print(r.gx); Serial.print(','); Serial.print(r.gy); Serial.print(','); Serial.print(r.gz); Serial.print(',');
+        Serial.print(r.ax); Serial.print(','); Serial.print(r.ay); Serial.print(','); Serial.print(r.az); Serial.print(',');
+        Serial.println((int)lroundf(motionDebug().rollDeg * 10.0f));
+      }
     }
-  #endif
+    if (n > 0) lastSample = samples[n - 1];
+    statSamples += n;
+    if (motionConsumeMoved()) lastMotionMs = now;
+  }
 
-  // Gyro reads with optional temperature compensation.
-  float correction = 0.0f;
-  #ifdef FEATURE_TEMP_COMPENSATION
-    if (cfg.featureFlags & FLAG_TEMP_COMP_ENABLED)
-      correction = TEMP_COMP_COEFF_DPS_C * (cachedTempC - calTempC);
-  #endif
-  float gx = (imu.readFloatGyroX() - biasGX) * (PI/180.0f);             // roll (unused for cursor)
-  float gy = (imu.readFloatGyroY() - biasGY - correction) * (PI/180.0f); // pitch → cursor Y
-  float gz = (imu.readFloatGyroZ() - biasGZ - correction) * (PI/180.0f); // yaw   → cursor X
+  // HID report, paced just under the connection interval: every radio event gets
+  // a report (an occasional event carries two), none goes out empty. Counts that
+  // could not be sent stay in the accumulator, so no motion is ever dropped.
+  static int32_t  streamMoveX = 0, streamMoveY = 0;
+  static uint32_t lastHidUs = 0;
+  {
+    bool conn = Bluefruit.connected();
+    uint32_t intervalUs = 10000;
+    if (conn) {
+      BLEConnection* c = Bluefruit.Connection(Bluefruit.connHandle());
+      if (c) intervalUs = c->getConnectionInterval() * 1250UL;
+    }
+    intervalUs = intervalUs * 7 / 8;
+    uint32_t nowUs = micros();
+    int8_t dx, dy;
+    if (nowUs - lastHidUs >= intervalUs && motionTakeCounts(dx, dy)) {
+      uint32_t gap = nowUs - lastHidUs;
+      lastHidUs = nowUs;
+      bool ok = !conn || blehid.mouseMove(dx, dy);  // not connected: counts are discarded
+      if (ok) {
+        statHidReports++; statSentX += dx; statSentY += dy;
+        if (gap > statMaxHidGapUs && gap < 1000000UL) statMaxHidGapUs = gap;
+        streamMoveX += dx; streamMoveY += dy;
+        if (testRunning) {
+          testReports++;
+          if (dx > 0) testPosX += dx; else testNegX += dx;
+          if (dy > 0) testPosY += dy; else testNegY += dy;
+        }
+      } else {
+        motionReturnCounts(dx, dy); statHidFails++; testFails++;
+      }
+    }
+  }
 
-  // Axis diagnostic — send 'd' over serial to enable
+  if (testRunning && !motionTestActive()) {
+    testRunning = false;
+    Serial.print("[TEST] done: +X="); Serial.print(testPosX); Serial.print(" -X="); Serial.print(testNegX);
+    Serial.print(" +Y="); Serial.print(testPosY); Serial.print(" -Y="); Serial.print(testNegY);
+    Serial.print(" reports="); Serial.print(testReports); Serial.print(" fails="); Serial.println(testFails);
+  }
+
+  if (statsEnabled) {
+    static unsigned long lastStat = 0;
+    if (now - lastStat >= 1000) { lastStat = now; printStats(now); }
+  }
+
+  // Axis diagnostic
   if (diagUntil && now < diagUntil) {
     static unsigned long lastDiagPrint = 0;
     if (now - lastDiagPrint >= 100) { lastDiagPrint = now;
-      Serial.print("[DIAG] gx="); Serial.print(gx,3);
-      Serial.print(" gy="); Serial.print(gy,3);
-      Serial.print(" gz="); Serial.println(gz,3);
+      const MotionDebug& m = motionDebug();
+      Serial.print("[DIAG] yaw="); Serial.print(m.yaw, 3);
+      Serial.print(" pitch="); Serial.print(m.pitch, 3);
+      Serial.print(" roll="); Serial.print(m.rollDeg, 1);
+      Serial.print(" speed="); Serial.println(m.speed, 4);
     }
   } else if (diagUntil) { diagUntil = 0; Serial.println("[DIAG] Done"); }
 
-  // Direct axis mapping (empirically verified via diagnostic)
-  float yawRate   =  gz;  // gyroZ = pan left/right → cursor X
-  float pitchRate =  gy;  // gyroY = nod up/down    → cursor Y
-
-  // Dead zone (equal for both axes)
-  float fYaw   = (fabsf(yawRate)   > cfg.deadZone) ? yawRate   : 0.0f;
-  float fPitch = (fabsf(pitchRate) > cfg.deadZone) ? pitchRate : 0.0f;
-
-  #ifdef DEBUG
-  { static unsigned long lastDiag = 0;
-    if (now - lastDiag >= 500) { lastDiag = now;
-      Serial.print("[IMU] gyro="); Serial.print(gx,2); Serial.print(","); Serial.print(gy,2); Serial.print(","); Serial.print(gz,2);
-      Serial.print(" yaw="); Serial.print(yawRate,3); Serial.print(" pitch="); Serial.println(pitchRate,3);
-    }
-  }
-  #endif
-
-  bool moving = (fPitch != 0.0f || fYaw != 0.0f);
-  if  (moving) { idleFrames = 0; idleStartMs = 0; }
-  else         { idleFrames++; if (idleStartMs == 0) idleStartMs = now; }
-  bool idle = (idleFrames >= IDLE_FRAMES);
-
-  #ifdef FEATURE_AUTO_RECAL
-    if ((cfg.featureFlags & FLAG_AUTO_RECAL_ENABLED) && idle && idleStartMs != 0 && (now - idleStartMs >= AUTO_RECAL_MS)) {
-      Serial.println("[AUTO-CAL] Long idle - recalibrating...");
-      idleStartMs = 0; calibrateGyroBias(); return;
-    }
-  #endif
-
-  int8_t  moveX = 0, moveY = 0;
-  uint8_t flags = 0;
-
-  static float smoothX = 0.0f, smoothY = 0.0f;
-
-  if (idle) {
-    smoothX = smoothY = 0.0f;
-    accumX = accumY = 0.0f;
-    flags |= 0x01;
-  } else {
-    float rawX = applyAcceleration(applyCurve(-fYaw   * cfg.sensitivity * dt));
-    float rawY = applyAcceleration(applyCurve(-fPitch * cfg.sensitivity * dt));
-    if (cfg.axisFlip & 0x01) rawX = -rawX;
-    if (cfg.axisFlip & 0x02) rawY = -rawY;
-    // Single-pole low-pass smoothing
-    smoothX = smoothX * (1.0f - SMOOTH_ALPHA) + rawX * SMOOTH_ALPHA;
-    smoothY = smoothY * (1.0f - SMOOTH_ALPHA) + rawY * SMOOTH_ALPHA;
-    accumX += smoothX; accumY += smoothY;
-    moveX = (int8_t)constrain((int)accumX, -127, 127);
-    moveY = (int8_t)constrain((int)accumY, -127, 127);
-    accumX -= moveX; accumY -= moveY;
-    if (Bluefruit.connected() && (moveX != 0 || moveY != 0)) blehid.mouseMove(moveX, moveY);
-  }
-
   #ifdef FEATURE_IMU_STREAM
     if (!safeMode && imuStreamEnabled && Bluefruit.connected()
         && (now - lastImuStream >= IMU_STREAM_RATE_MS)) {
@@ -436,18 +520,21 @@ void loop() {
       if (now < streamBackoffUntil) {
         // Backing off - host TX buffer congested, skip to avoid 100ms block
       } else {
-        float ax = imu.readFloatAccelX(), ay = imu.readFloatAccelY(), az = imu.readFloatAccelZ();
+        const MotionDebug& m = motionDebug();
         ImuPacket pkt;
-        pkt.gyroX_mDPS = (int16_t)constrain(gx*(180.f/PI)*1000.f, -32000, 32000);
-        pkt.gyroZ_mDPS = (int16_t)constrain(gz*(180.f/PI)*1000.f, -32000, 32000);
-        pkt.accelX_mg  = (int16_t)constrain(ax*1000.f, -32000, 32000);
-        pkt.accelY_mg  = (int16_t)constrain(ay*1000.f, -32000, 32000);
-        pkt.accelZ_mg  = (int16_t)constrain(az*1000.f, -32000, 32000);
-        pkt.moveX      = moveX;
-        pkt.moveY      = moveY;
-        pkt.flags      = flags;
+        int8_t mx = (int8_t)constrain(streamMoveX, -127, 127);
+        int8_t my = (int8_t)constrain(streamMoveY, -127, 127);
+        pkt.gyroX_mDPS = (int16_t)constrain(m.pitch * (180.f/PI) * 1000.f, -32000, 32000);
+        pkt.gyroZ_mDPS = (int16_t)constrain(m.yaw   * (180.f/PI) * 1000.f, -32000, 32000);
+        pkt.accelX_mg  = (int16_t)constrain(lastSample.ax * ACCEL_G_PER_LSB * 1000.f, -32000, 32000);
+        pkt.accelY_mg  = (int16_t)constrain(lastSample.ay * ACCEL_G_PER_LSB * 1000.f, -32000, 32000);
+        pkt.accelZ_mg  = (int16_t)constrain(lastSample.az * ACCEL_G_PER_LSB * 1000.f, -32000, 32000);
+        pkt.moveX      = mx;
+        pkt.moveY      = my;
+        pkt.flags      = idle ? 0x01 : 0x00;
         pkt._pad       = 0;
         if (cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt))) {
+          streamMoveX -= mx; streamMoveY -= my;
           streamNotifyOk++;
           streamConsecFails = 0;
         } else {
@@ -471,5 +558,6 @@ void loop() {
         streamNotifyOk = 0; streamNotifyFails = 0;
       }
     }
+    if (!imuStreamEnabled) streamMoveX = streamMoveY = 0;
   #endif
-}
+}

source/motion.cpp

@@ -0,0 +1,267 @@
+#include "motion.h"
+#include "buttons.h"
+#include <math.h>
+
+// Tuning
+// Stillness / bias tracking: the gyro is "at rest" when every axis stays inside
+// STILL_PP_LSB peak-to-peak for a whole window. Hand tremor alone is far above this,
+// so the bias only updates when the remote is put down.
+static const int   STILL_WIN          = 104;                  // samples (0.5 s)
+static const int   STILL_PP_LSB       = 16;                   // 0.56 dps
+static const int   STILL_WINS_UPDATE  = 2;                    // 1 s at rest before updating
+static const int   STILL_WINS_JUMP    = 6;                    // 3 s at rest if the bias moved a lot
+static const float BIAS_JUMP_RAD      = 0.5f * (PI / 180.0f);
+static const float BIAS_UPDATE_K      = 0.3f;
+
+// Gravity estimate: gyro-propagated, pulled toward the accelerometer with this
+// time constant, only while |a| is close to 1 g (no hand acceleration).
+static const float GRAV_TAU_S         = 1.0f;
+static const float GRAV_GATE_G        = 0.15f;
+static const float ROLL_MIN_HORIZ     = 0.25f;  // below this the remote points ~straight up/down
+static const float ROLL_REANCHOR_UP   = 0.5f;   // button face within 60 deg of up: gravity is unambiguous
+
+// Speed-adaptive smoothing (One Euro style): cutoff = MIN + BETA * speed.
+static const float SMOOTH_MIN_HZ      = 1.5f;
+static const float SMOOTH_BETA        = 20.0f;  // Hz per rad/s
+static const float SMOOTH_MAX_HZ      = 40.0f;
+
+static const float IDLE_RATE_RAD      = 0.06f;  // sleep/idle detection, raw rate
+static const unsigned long CLICK_FREEZE_MS = 120;  // hold cursor around button edges
+
+static const float TEST_RATE_RAD      = 0.5f;
+static const int   TEST_SEG_SAMPLES   = (int)IMU_ODR_HZ;  // 1 s per segment
+
+// State
+static float bias[3]   = {0, 0, 0};
+static bool  biasValid = false;
+static float up[3]     = {0, 0, 1};
+static bool  upSeeded  = false;
+static float rollC = 1.0f, rollS = 0.0f;
+static bool  rollFromGravity = true;  // next roll is taken from gravity as-is (no continuity)
+static float filtX = 0, filtY = 0;  // smoothed yaw/pitch, rad/s
+static float accX  = 0, accY  = 0;  // cursor counts not yet sent
+static bool  moved = false;
+
+static int     winN = 0;
+static int32_t winSum[3];
+static int16_t winMin[3], winMax[3];
+static int     stillWins = 0;
+
+static int testSample = -1;
+static MotionDebug dbg = {};
+
+static void resetWindow() {
+  winN = 0;
+  for (int i = 0; i < 3; i++) { winSum[i] = 0; winMin[i] = 32767; winMax[i] = -32768; }
+}
+
+static void publishBiasStats() {
+  #ifdef FEATURE_TELEMETRY
+    statBiasRms = sqrtf((bias[0]*bias[0] + bias[1]*bias[1] + bias[2]*bias[2]) / 3.0f);
+  #endif
+}
+
+void motionReset() {
+  filtX = filtY = 0;
+  accX = accY = 0;
+  upSeeded = false;
+  rollFromGravity = true;
+  stillWins = 0;
+  resetWindow();
+}
+
+bool motionApplyCalibration(const float b[3], const float a[3], bool still) {
+  bool apply = still || !biasValid;
+  if (apply) {
+    for (int i = 0; i < 3; i++) bias[i] = b[i];
+    biasValid = still;
+    publishBiasStats();
+  }
+  motionReset();
+  float an = sqrtf(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
+  if (an > 0.5f) {
+    for (int i = 0; i < 3; i++) up[i] = a[i] / an;
+    upSeeded = true;
+  }
+  return apply;
+}
+
+// Continuous bias tracking while the remote is at rest.
+static void trackBias(const ImuRaw& r) {
+  const int16_t g[3] = {r.gx, r.gy, r.gz};
+  for (int i = 0; i < 3; i++) {
+    winSum[i] += g[i];
+    if (g[i] < winMin[i]) winMin[i] = g[i];
+    if (g[i] > winMax[i]) winMax[i] = g[i];
+  }
+  if (++winN < STILL_WIN) return;
+
+  int pp = 0;
+  float mean[3], jump = 0;
+  for (int i = 0; i < 3; i++) {
+    pp = max(pp, winMax[i] - winMin[i]);
+    mean[i] = (float)winSum[i] / winN * GYRO_RAD_PER_LSB;
+    jump = max(jump, fabsf(mean[i] - bias[i]));
+  }
+  resetWindow();
+
+  bool still = pp <= STILL_PP_LSB;
+  dbg.still = still;
+  stillWins = still ? stillWins + 1 : 0;
+  if (stillWins >= STILL_WINS_UPDATE) rollFromGravity = true;  // put down: re-take roll from gravity
+  int needed = (biasValid && jump > BIAS_JUMP_RAD) ? STILL_WINS_JUMP : STILL_WINS_UPDATE;
+  if (stillWins < needed) return;
+
+  float k = biasValid ? BIAS_UPDATE_K : 1.0f;
+  for (int i = 0; i < 3; i++) bias[i] += k * (mean[i] - bias[i]);
+  biasValid = true;
+  calTempC = cachedTempC;  // temperature compensation is relative to the last bias update
+  dbg.biasUpdates++;
+  publishBiasStats();
+}
+
+static void updateGravity(const float w[3], const float a[3]) {
+  float an = sqrtf(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
+  if (!upSeeded) {
+    if (an < 0.5f) return;
+    for (int i = 0; i < 3; i++) up[i] = a[i] / an;
+    upSeeded = true;
+    return;
+  }
+  // A world-fixed vector seen from the body rotates as du/dt = u x w.
+  float ux = up[0] + (up[1]*w[2] - up[2]*w[1]) * IMU_DT;
+  float uy = up[1] + (up[2]*w[0] - up[0]*w[2]) * IMU_DT;
+  float uz = up[2] + (up[0]*w[1] - up[1]*w[0]) * IMU_DT;
+
+  float err = fabsf(an - 1.0f);
+  if (an > 0.1f && err < GRAV_GATE_G) {
+    float k = (IMU_DT / GRAV_TAU_S) * (1.0f - err / GRAV_GATE_G);
+    ux += k * (a[0]/an - ux);
+    uy += k * (a[1]/an - uy);
+    uz += k * (a[2]/an - uz);
+  }
+  float n = sqrtf(ux*ux + uy*uy + uz*uz);
+  if (n > 1e-6f) { up[0] = ux/n; up[1] = uy/n; up[2] = uz/n; }
+}
+
+void motionProcess(const ImuRaw& r, unsigned long nowMs) {
+  dbg.samples++;
+
+  #ifdef FEATURE_AUTO_RECAL
+    if (cfg.featureFlags & FLAG_AUTO_RECAL_ENABLED) trackBias(r);
+  #endif
+
+  float w[3] = { r.gx * GYRO_RAD_PER_LSB - bias[0],
+                 r.gy * GYRO_RAD_PER_LSB - bias[1],
+                 r.gz * GYRO_RAD_PER_LSB - bias[2] };
+  #ifdef FEATURE_TEMP_COMPENSATION
+    if (cfg.featureFlags & FLAG_TEMP_COMP_ENABLED) {
+      float corr = TEMP_COMP_COEFF_DPS_C * (cachedTempC - calTempC) * (PI / 180.0f);
+      w[1] -= corr; w[2] -= corr;
+    }
+  #endif
+  const float a[3] = { r.ax * ACCEL_G_PER_LSB, r.ay * ACCEL_G_PER_LSB, r.az * ACCEL_G_PER_LSB };
+  updateGravity(w, a);
+
+  // Roll compensation. Body frame in the enclosure: +X toward the wrist (tip is -X),
+  // +Y right, +Z out of the button face. Twisting the remote is a rotation about X,
+  // i.e. of the (gy, gz) plane. Rotate it back using gravity:
+  //   yaw   = w . n, n = (0, uy, uz)/|.|   (de-rolled "up")
+  //   pitch = w . r, r = (0, uz, -uy)/|.|  (horizontal, across the pointing axis)
+  // Held flat this is exactly yaw=gz, pitch=gy. Roll rate (gx) never reaches the cursor.
+  //
+  // Gravity alone can't tell "tilted back past vertical" from "held upside down":
+  // both put the button face below the horizon. Stay on the branch closest to the
+  // previous roll, so tilting through vertical never flips the axes, while a real
+  // twist (always continuous) is still followed. The branch is re-taken from gravity
+  // whenever tracking restarts, the remote is put down, or the button face points
+  // roughly up: that pose is never ambiguous, so whatever happened during odd
+  // movements, a normal grip always tracks normally again.
+  // Near vertical gravity says nothing about twist, so there the twist is followed by
+  // integrating the gyro's rate around the pointing axis; otherwise spinning the
+  // remote while it points up would leave it on the wrong branch afterwards.
+  float horiz = sqrtf(up[1]*up[1] + up[2]*up[2]);
+  if (horiz > ROLL_MIN_HORIZ) {
+    float c = up[2] / horiz, s = up[1] / horiz;
+    bool anchored = rollFromGravity || up[2] > ROLL_REANCHOR_UP;
+    if (!anchored && c*rollC + s*rollS < 0.0f) { c = -c; s = -s; }
+    rollC = c; rollS = s;
+    rollFromGravity = false;
+  } else {
+    float d = w[0] * IMU_DT, cd = cosf(d), sd = sinf(d);
+    float c = rollC*cd - rollS*sd, s = rollS*cd + rollC*sd;
+    float n = sqrtf(c*c + s*s);
+    rollC = c / n; rollS = s / n;
+  }
+  float yaw   = w[1]*rollS + w[2]*rollC;
+  float pitch = w[1]*rollC - w[2]*rollS;
+
+  if (testSample >= 0) {
+    int seg = testSample / TEST_SEG_SAMPLES;
+    yaw = pitch = 0;
+    if      (seg == 0) yaw   = -TEST_RATE_RAD;  // cursor right
+    else if (seg == 1) yaw   =  TEST_RATE_RAD;  // cursor left
+    else if (seg == 2) pitch = -TEST_RATE_RAD;  // cursor down
+    else if (seg == 3) pitch =  TEST_RATE_RAD;  // cursor up
+    if (++testSample >= 5 * TEST_SEG_SAMPLES) testSample = -1;  // 5th second lets the filter settle
+  }
+
+  dbg.yaw = yaw; dbg.pitch = pitch;
+  if (yaw*yaw + pitch*pitch > IDLE_RATE_RAD*IDLE_RATE_RAD || testSample >= 0) moved = true;
+
+  // Speed-adaptive low-pass: tremor and sensor noise are removed at low speed,
+  // while fast movements pass with almost no lag.
+  float sPrev = sqrtf(filtX*filtX + filtY*filtY);
+  float fc = min(SMOOTH_MIN_HZ + SMOOTH_BETA * sPrev, SMOOTH_MAX_HZ);
+  float alpha = 1.0f / (1.0f + 1.0f / (2.0f * PI * fc * IMU_DT));
+  filtX += alpha * (yaw   - filtX);
+  filtY += alpha * (pitch - filtY);
+
+  // Radial soft dead zone: gain s^2/(s^2+dz^2) is continuous (no jump when motion
+  // starts) and acts on the speed, so diagonal motion keeps its direction.
+  float s  = sqrtf(filtX*filtX + filtY*filtY);
+  float dz = cfg.deadZone;
+  float g  = (s > 1e-9f) ? (s*s) / (s*s + dz*dz) : 0.0f;
+  float sv = s * g;
+  dbg.speed = sv;
+  if (sv < 1e-9f) return;
+
+  // Curve and acceleration act on speed, expressed as counts per 10 ms so existing
+  // settings keep their feel, then scale both axes equally.
+  float dref  = cfg.sensitivity * sv * 0.01f;
+  float m     = applyAcceleration(applyCurve(dref));
+  float scale = (m / dref) * cfg.sensitivity * IMU_DT * g;
+  float cx = -filtX * scale;
+  float cy = -filtY * scale;
+  if (cfg.axisFlip & 0x01) cx = -cx;
+  if (cfg.axisFlip & 0x02) cy = -cy;
+
+  #ifdef FEATURE_PHYSICAL_BUTTONS
+    // Pressing a button twists the remote slightly; hold the cursor still around the click.
+    if (btnLastEdgeMs && (nowMs - btnLastEdgeMs) < CLICK_FREEZE_MS && testSample < 0) return;
+  #endif
+  accX += cx; accY += cy;
+}
+
+bool motionTakeCounts(int8_t& dx, int8_t& dy) {
+  int ix = constrain((int)accX, -127, 127);
+  int iy = constrain((int)accY, -127, 127);
+  if (ix == 0 && iy == 0) return false;
+  accX -= ix; accY -= iy;
+  dx = (int8_t)ix; dy = (int8_t)iy;
+  return true;
+}
+
+void motionReturnCounts(int8_t dx, int8_t dy) { accX += dx; accY += dy; }
+
+bool motionConsumeMoved() { bool m = moved; moved = false; return m; }
+
+void motionStartTest() { testSample = 0; filtX = filtY = 0; accX = accY = 0; }
+bool motionTestActive() { return testSample >= 0; }
+
+const MotionDebug& motionDebug() {
+  for (int i = 0; i < 3; i++) { dbg.up[i] = up[i]; dbg.bias[i] = bias[i]; }
+  dbg.biasValid = biasValid;
+  dbg.rollDeg = atan2f(rollS, rollC) * (180.0f / PI);
+  return dbg;
+}

source/motion.h

@@ -0,0 +1,32 @@
+#pragma once
+#include "config.h"
+#include "imu.h"
+
+// Tracking pipeline, run once per IMU sample:
+//   bias removal -> gravity estimate (gyro-propagated, accel-corrected)
+//   -> roll compensation (cursor follows world yaw/pitch however the remote is twisted)
+//   -> speed-adaptive low-pass (heavy at rest, none when moving fast)
+//   -> radial soft dead zone -> curve/acceleration on speed (direction preserved)
+//   -> sub-count accumulator drained by the HID report scheduler.
+
+struct MotionDebug {
+  float yaw, pitch;      // roll-compensated rates, rad/s (unfiltered)
+  float speed;           // smoothed speed after dead zone, rad/s
+  float up[3];           // gravity-up unit vector, device frame
+  float rollDeg;         // twist around the pointing axis
+  float bias[3];         // rad/s
+  bool  biasValid;
+  bool  still;           // last stillness window passed
+  uint32_t biasUpdates;
+  uint32_t samples;
+};
+
+void motionReset();
+bool motionApplyCalibration(const float bias[3], const float accel[3], bool still);
+void motionProcess(const ImuRaw& s, unsigned long nowMs);
+bool motionTakeCounts(int8_t& dx, int8_t& dy);
+void motionReturnCounts(int8_t dx, int8_t dy);
+bool motionConsumeMoved();      // true if any sample since the last call was above the idle rate
+void motionStartTest();         // synthetic right/left/down/up sweep for end-to-end testing
+bool motionTestActive();
+const MotionDebug& motionDebug();

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;

source/tap.cpp

@@ -7,7 +7,7 @@
 extern BLEHidAdafruit blehid;
 
 // Tap detection setup 
-// REG_TAP_THS_6D bits[4:0] = tapThreshold (1–31); 1 LSB = FS/32 = 62.5 mg at ±2g.
+// REG_TAP_THS_6D bits[4:0] = tapThreshold (1–31); 1 LSB = FS/32 = 125 mg at ±4g.
 // REG_INT_DUR2 at ODR=416 Hz:
 //  SHOCK[7:6] = 2 → 38 ms max tap duration
 //  QUIET[5:4] = 2 → 19 ms refractory after tap
@@ -20,14 +20,15 @@ void applyTapThreshold() {
 }
 
 void setupTapDetection() {
-  imuWriteReg(REG_CTRL1_XL,    0x60);  // ODR=416 Hz, FS=±2g
+  // ODR=416 Hz; keep the ±4 g full scale the tracking pipeline expects
+  imuWriteReg(REG_CTRL1_XL, (imuReadReg(REG_CTRL1_XL) & 0x0F) | 0x60);
   imuWriteReg(REG_TAP_CFG,     0x8E);  // TIMER_EN + LIR + TAP_Z/Y/X enabled
   applyTapThreshold();
   imuWriteReg(REG_INT_DUR2,    0x62);  // SHOCK=2(38ms), QUIET=2(19ms), DUR=6(115ms)
   imuWriteReg(REG_WAKE_UP_THS, 0x80);  // bit7=1 → single + double tap both enabled
   imuWriteReg(REG_MD1_CFG,     0x48);  // route single-tap(0x08) + double-tap(0x40) → INT1
   Serial.print("[TAP] threshold="); Serial.print(cfg.tapThreshold);
-  Serial.print(" (~"); Serial.print(cfg.tapThreshold * 62.5f, 0); Serial.println(" mg)");
+  Serial.print(" (~"); Serial.print(cfg.tapThreshold * 125.0f, 0); Serial.println(" mg)");
 }
 
 // Tap processing