diff --git a/README.md b/README.md index 8af9fec..295bd6e 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ A BLE HID mouse that uses the onboard IMU of a **Seeed XIAO nRF52840 Sense** to ## Features -- **6-DoF gyro + accelerometer** via LSM6DS3 with complementary filter +- **Roll-compensated pointing** - gyro + accelerometer fusion keeps the cursor level however the remote is twisted in the hand - **Hardware tap detection** - single tap = left click, double tap = right click - **BLE HID mouse** - works natively on Windows, macOS, Linux, Android, iOS - **BLE Battery Service** - charge level visible in OS Bluetooth settings @@ -13,7 +13,7 @@ A BLE HID mouse that uses the onboard IMU of a **Seeed XIAO nRF52840 Sense** to - **Live IMU stream** - 20 Hz gyro/accel data streamed to the web UI visualiser - **Live telemetry** - temperature, uptime, click counts, gyro bias RMS, recal count - **Temperature compensation** - gyro drift correction by Δ temperature since last calibration -- **Auto-recalibration** - recalibrates automatically after 5 minutes of idle +- **Continuous bias tracking** - gyro bias is re-estimated whenever the remote is at rest, so the cursor does not drift - **Configurable charge rate** - OFF / 50 mA slow / 100 mA fast via BQ25100 HICHG pin - **Boot-loop detection** - 3 rapid reboots trigger safe mode (config service disabled, flash wiped) @@ -48,7 +48,7 @@ Open `web/index.html` in Chrome or Edge (desktop). Requires Web Bluetooth - enab | Parameter | Range | Description | |---|---|---| | Sensitivity | 100 – 1500 | Cursor speed multiplier | -| Dead zone | 0.005 – 0.2 rad/s | Noise floor; raise to reduce drift | +| Dead zone | 0 – 0.1 rad/s | Soft dead zone on smoothed speed; raise to steady the cursor while holding still | | Accel strength | 0 – 0.5 | Pointer acceleration multiplier | | Curve | Linear / Square / √Sqrt | Response shape for input magnitude | | Flip X / Y | on/off | Invert horizontal or vertical axis | @@ -59,6 +59,18 @@ Open `web/index.html` in Chrome or Edge (desktop). Requires Web Bluetooth - enab - **Calibrate Gyro** - recalculates bias offset; hold the device still on a flat surface for ~1 s - **Factory Reset** - wipes flash config, restores defaults +**Serial commands** (115200 baud over USB): + +| Key | Action | +|---|---| +| `c` | Calibrate gyro | +| `r` | Factory reset | +| `s` | Toggle 1 Hz tracking stats (sample rate, HID reports, bias, roll, BLE interval); keeps the device awake | +| `l` | Toggle raw IMU log, one CSV line per sample; keeps the device awake | +| `d` | 10 s of roll-compensated yaw/pitch rates and roll angle | +| `t` | Synthetic sweep (right, left, down, up); moves the host cursor and prints the counts sent | +| `o` | Enter OTA DFU mode | + ## Building ### Requirements @@ -84,7 +96,7 @@ All optional features are enabled by `#define` in `source/config.h`. Comment out #define FEATURE_IMU_STREAM // 20 Hz raw IMU stream #define FEATURE_TAP_DETECTION // Hardware tap → click engine #define FEATURE_TEMP_COMPENSATION // Gyro drift correction by temperature -#define FEATURE_AUTO_RECAL // Auto-recalibrate after 5 min idle +#define FEATURE_AUTO_RECAL // Continuous gyro bias tracking while at rest #define FEATURE_BATTERY_MONITOR // ADC battery level + BLE Battery Service #define FEATURE_BOOT_LOOP_DETECT // Crash counter → safe mode ``` diff --git a/source/buttons.cpp b/source/buttons.cpp index 60e97a5..ca4208b 100644 --- a/source/buttons.cpp +++ b/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; diff --git a/source/buttons.h b/source/buttons.h index 485d148..41706b3 100644 --- a/source/buttons.h +++ b/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 diff --git a/source/config.h b/source/config.h index 63626bf..8a202b4 100644 --- a/source/config.h +++ b/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; diff --git a/source/imu.cpp b/source/imu.cpp index 2adb88d..f11fb6c 100644 --- a/source/imu.cpp +++ b/source/imu.cpp @@ -1,10 +1,28 @@ #include "imu.h" +#include "motion.h" #include "Wire.h" #include 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= 0 ? 1.f : -1.f) * v * v; diff --git a/source/imu.h b/source/imu.h index 74aea5b..e5a0249 100644 --- a/source/imu.h +++ b/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); diff --git a/source/main.cpp b/source/main.cpp index fcb857a..4c1d39d 100644 --- a/source/main.cpp +++ b/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 -} \ No newline at end of file +} diff --git a/source/motion.cpp b/source/motion.cpp new file mode 100644 index 0000000..f78f5cf --- /dev/null +++ b/source/motion.cpp @@ -0,0 +1,267 @@ +#include "motion.h" +#include "buttons.h" +#include + +// 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; +} diff --git a/source/motion.h b/source/motion.h new file mode 100644 index 0000000..7fe3532 --- /dev/null +++ b/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(); diff --git a/source/sleep.cpp b/source/sleep.cpp index 1e2585e..173a0a7 100644 --- a/source/sleep.cpp +++ b/source/sleep.cpp @@ -1,5 +1,6 @@ #include "sleep.h" #include "imu.h" // config.h REG_* macros +#include "motion.h" #include // 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; diff --git a/source/tap.cpp b/source/tap.cpp index 6e78171..e0948cc 100644 --- a/source/tap.cpp +++ b/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 diff --git a/web/app.js b/web/app.js index 77b16cd..8270f6c 100644 --- a/web/app.js +++ b/web/app.js @@ -16,7 +16,7 @@ const FLAG_AUTO_RECAL_ENABLED = 0x04; const FLAG_ALL_DEFAULT = FLAG_TAP_ENABLED | FLAG_TEMP_COMP_ENABLED | FLAG_AUTO_RECAL_ENABLED; // Local shadow of the current config (kept in sync with device) -const config = { sensitivity:600, deadZone:0.06, accelStrength:0.08, curve:0, axisFlip:0, chargeMode:0, +const config = { sensitivity:600, deadZone:0.02, accelStrength:0.08, curve:0, axisFlip:0, chargeMode:0, tapThreshold:12, tapAction:0, tapKey:0, tapMod:0, tapFreezeEnabled:1, jerkThreshold:2000, featureFlags:FLAG_ALL_DEFAULT, }; @@ -601,7 +601,7 @@ function updateDisplay(key, val) { deadZone: ['valDeadZone', v=>parseFloat(v).toFixed(3)], accel: ['valAccel', v=>parseFloat(v).toFixed(2)], jerkThreshold:['valJerkThreshold',v=>parseFloat(v).toFixed(0)], - tapThreshold: ['valTapThreshold', v=>(parseFloat(v)*62.5).toFixed(0)+' mg'], + tapThreshold: ['valTapThreshold', v=>(parseFloat(v)*125).toFixed(0)+' mg'], }; const [id,fmt] = map[key]; document.getElementById(id).textContent = fmt(val); diff --git a/web/index.html b/web/index.html index ce448a9..143ad0f 100644 --- a/web/index.html +++ b/web/index.html @@ -51,10 +51,10 @@
600
-
Dead Zone
Noise floor (rad/s) - raise to reduce drift
-
Dead Zone
Soft dead zone (rad/s) - raise to steady the cursor while holding still
+ -
0.060
+
0.020
Accel Strength
Pointer acceleration multiplier
@@ -126,7 +126,7 @@
Auto Recalibration
-
Recalibrate gyro after long idle period
+
Re-estimate gyro bias whenever the remote is at rest