diff --git a/source/config.h b/source/config.h index f7b0c65..c7cad71 100644 --- a/source/config.h +++ b/source/config.h @@ -158,8 +158,7 @@ struct __attribute__((packed)) ImuPacket { static_assert(sizeof(ImuPacket) == 14, "ImuPacket must be 14 bytes"); #endif -// Tuning constants -extern const float ALPHA; +// Tuning constants extern const int LOOP_RATE_MS; extern const int BIAS_SAMPLES; extern const int IDLE_FRAMES; @@ -184,10 +183,8 @@ extern const float BATT_CRITICAL; extern const unsigned long AUTO_RECAL_MS; #endif -// Global state -extern float angleX, angleY; +// Global state extern float accumX, accumY; -extern float gravX, gravY, gravZ; extern float biasGX, biasGY, biasGZ; extern float calTempC; extern float cachedTempC; diff --git a/source/imu.cpp b/source/imu.cpp index 3341827..2adb88d 100644 --- a/source/imu.cpp +++ b/source/imu.cpp @@ -34,9 +34,7 @@ void calibrateGyroBias() { biasGY = (float)(sy/BIAS_SAMPLES); biasGZ = (float)(sz/BIAS_SAMPLES); calTempC = readIMUTemp(); - angleX = angleY = accumX = accumY = 0.0f; - // Seed gravity estimate from current accel so projection is correct immediately - gravX = imu.readFloatAccelX(); gravY = imu.readFloatAccelY(); gravZ = imu.readFloatAccelZ(); + accumX = accumY = 0.0f; #ifdef FEATURE_TELEMETRY statRecalCount++; diff --git a/source/main.cpp b/source/main.cpp index 722bd37..0588ec2 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -55,11 +55,9 @@ const Config CFG_DEFAULTS = { TelemetryPacket telem = {}; #endif -// Tuning constants -const float ALPHA = 0.96f; +// Tuning constants const int LOOP_RATE_MS = 10; -const float SMOOTH_LOW_RPS = 0.15f; // below this → heavy EMA smoothing (~8°/s) -const float SMOOTH_HIGH_RPS = 0.50f; // above this → no smoothing (~29°/s) +const float SMOOTH_ALPHA = 0.65f; // single-pole low-pass for cursor smoothing const int BIAS_SAMPLES = 200; const int IDLE_FRAMES = 150; const unsigned long BATT_REPORT_MS = 20000; @@ -83,11 +81,8 @@ const float BATT_CRITICAL = 3.10f; const unsigned long AUTO_RECAL_MS = 5UL * 60UL * 1000UL; #endif -// Global state definitions -float angleX = 0, angleY = 0; +// Global state definitions float accumX = 0, accumY = 0; -// Low-pass filtered gravity estimate in device frame (for roll-independent axis projection) -float gravX = 0, gravY = 0, gravZ = 1.0f; float biasGX = 0, biasGY = 0, biasGZ = 0; float calTempC = 25.0f; float cachedTempC = 25.0f; @@ -297,11 +292,13 @@ void loop() { } #endif - // Serial commands: 'c' = calibrate, 'r' = factory reset + // Serial commands: 'c' = calibrate, 'r' = factory reset, 'd' = axis diagnostic + static unsigned long diagUntil = 0; 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; } #ifdef FEATURE_OTA if (cmd == 'o') { Serial.println("[SERIAL] OTA DFU"); pendingOTA = true; } #endif @@ -371,14 +368,27 @@ void loop() { if (cfg.featureFlags & FLAG_TEMP_COMP_ENABLED) correction = TEMP_COMP_COEFF_DPS_C * (cachedTempC - calTempC); #endif - float gx = (imu.readFloatGyroX() - biasGX - correction) * (PI/180.0f); - float gy = (imu.readFloatGyroY() - biasGY - correction) * (PI/180.0f); - float gz = (imu.readFloatGyroZ() - biasGZ - correction) * (PI/180.0f); + 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 float ax = imu.readFloatAccelX(); float ay = imu.readFloatAccelY(); float az = imu.readFloatAccelZ(); + // Axis diagnostic — send 'd' over serial to enable + 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.print(gz,3); + Serial.print(" | ax="); Serial.print(ax,3); + Serial.print(" ay="); Serial.print(ay,3); + Serial.print(" az="); Serial.println(az,3); + } + } else if (diagUntil) { diagUntil = 0; Serial.println("[DIAG] Done"); } + // Jerk-based shock detection - freeze cursor during tap impacts, doesn't work well yet float jx = (ax - prevAx) / dt, jy = (ay - prevAy) / dt, jz = (az - prevAz) / dt; float jerkSq = jx*jx + jy*jy + jz*jz; @@ -386,47 +396,18 @@ void loop() { bool shocked = cfg.tapFreezeEnabled && ((jerkSq > cfg.jerkThreshold) || (now < shockFreezeUntil)); if (cfg.tapFreezeEnabled && jerkSq > cfg.jerkThreshold) shockFreezeUntil = now + SHOCK_FREEZE_MS; - // Complementary filter - if (shocked) { - angleX += gx * dt; - angleY += gz * dt; - } else { - angleX = ALPHA*(angleX + gx*dt) + (1.0f - ALPHA)*atan2f(ax, sqrtf(ay*ay + az*az)); - angleY = ALPHA*(angleY + gz*dt) + (1.0f - ALPHA)*atan2f(ay, sqrtf(ax*ax + az*az)); - } + // 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 - // Gravity-based axis decomposition - const float GRAV_LP = 0.05f; - if (!shocked) { - gravX += GRAV_LP * (ax - gravX); - gravY += GRAV_LP * (ay - gravY); - gravZ += GRAV_LP * (az - gravZ); - } - - float gN = sqrtf(gravX*gravX + gravY*gravY + gravZ*gravZ); - if (gN < 0.3f) gN = 1.0f; - float gnx = gravX/gN, gny = gravY/gN, gnz = gravZ/gN; - - float ry = -gnz, rz = gny; - float rN = sqrtf(ry*ry + rz*rz); - if (rN < 0.01f) { ry = -1.0f; rz = 0.0f; rN = 1.0f; } - ry /= rN; rz /= rN; - - // Yaw (cursor X) = angular velocity component around gravity (vertical) - // Pitch (cursor Y) = angular velocity component around screen-right - float yawRate = gx*gnx + gy*gny + gz*gnz; - float pitchRate = -(gy*ry + gz*rz); - - // Projected rates amplify residual gyro bias (especially GY drift on pitch axis). - float fYaw = (fabsf(yawRate) > cfg.deadZone) ? yawRate : 0.0f; - float fPitch = (fabsf(pitchRate) > cfg.deadZone * 3.0f) ? pitchRate : 0.0f; + // 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("[PROJ] grav="); Serial.print(gnx,2); Serial.print(","); Serial.print(gny,2); Serial.print(","); Serial.print(gnz,2); - Serial.print(" R="); Serial.print(ry,2); Serial.print(","); Serial.print(rz,2); - Serial.print(" gyro="); Serial.print(gx,2); Serial.print(","); Serial.print(gy,2); Serial.print(","); Serial.print(gz,2); + 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); } } @@ -463,13 +444,9 @@ void loop() { float rawY = applyAcceleration(applyCurve(-fPitch * cfg.sensitivity * dt)); if (cfg.axisFlip & 0x01) rawX = -rawX; if (cfg.axisFlip & 0x02) rawY = -rawY; - // Tiered velocity smoothing: heavy EMA when nearly still, none when fast. - // Thresholds are in rad/s (angular rate), independent of sensitivity setting. - float speed = sqrtf(fYaw*fYaw + fPitch*fPitch); - float alpha = (speed < SMOOTH_LOW_RPS) ? 0.25f : - (speed < SMOOTH_HIGH_RPS) ? 0.65f : 1.00f; - smoothX = smoothX * (1.0f - alpha) + rawX * alpha; - smoothY = smoothY * (1.0f - alpha) + rawY * alpha; + // 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); diff --git a/source/sleep.cpp b/source/sleep.cpp index a0dbd05..53b60c0 100644 --- a/source/sleep.cpp +++ b/source/sleep.cpp @@ -186,24 +186,11 @@ void sleepManagerWakeIMU() { lpEnteredMs = 0; // Reset motion filter state to prevent a cursor jump on the first frame. - // After sleep: angleX/Y are stale, gravX/Y/Z drifted, accumX/Y is dirty, - // and lastTime is old so dt would be huge on the first loop iteration. - // Zeroing these here means the first frame integrates 0 motion cleanly. - extern float angleX, angleY; extern float accumX, accumY; - extern float gravX, gravY, gravZ; extern float prevAx, prevAy, prevAz; extern unsigned long lastTime; - angleX = angleY = 0.0f; accumX = accumY = 0.0f; - // Reseed gravity from current accel so projection is correct immediately. - // Can't call imu.readFloat* here (gyro not fully settled), but accel is - // already running - read it directly via Wire1. - // Simpler: just reset to neutral [0,0,1] and let the LP filter converge - // over the first ~20 frames (200 ms) of real use. - gravX = 0.0f; gravY = 0.0f; gravZ = 1.0f; prevAx = 0.0f; prevAy = 0.0f; prevAz = 0.0f; - // Set lastTime to now so the first dt = 0 rather than (now - sleepEntryTime) lastTime = millis(); sleepStage = SLEEP_AWAKE;