diff --git a/source/config.h b/source/config.h index 5c23ae2..fb25e9d 100644 --- a/source/config.h +++ b/source/config.h @@ -164,6 +164,7 @@ extern const float BATT_CRITICAL; // ─── Global state ───────────────────────────────────────────────────────────── extern float angleX, angleY; 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 74e24ab..2b431ca 100644 --- a/source/imu.cpp +++ b/source/imu.cpp @@ -35,6 +35,8 @@ void calibrateGyroBias() { 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(); #ifdef FEATURE_TELEMETRY statRecalCount++; diff --git a/source/main.cpp b/source/main.cpp index 0677a68..2d06eed 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -96,6 +96,8 @@ const float BATT_CRITICAL = 3.10f; // ─── Global state definitions ───────────────────────────────────────────────── float angleX = 0, angleY = 0; 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; @@ -327,15 +329,15 @@ void loop() { #endif // Gyro reads with optional temperature compensation - float gx, gz; + float gx, gy, gz; #ifdef FEATURE_TEMP_COMPENSATION float correction = TEMP_COMP_COEFF_DPS_C * (cachedTempC - calTempC); gx = (imu.readFloatGyroX() - biasGX - correction) * (PI/180.0f); - (void)(imu.readFloatGyroY()); // GY unused — read to keep SPI/I2C sequence consistent + gy = (imu.readFloatGyroY() - biasGY - correction) * (PI/180.0f); gz = (imu.readFloatGyroZ() - biasGZ - correction) * (PI/180.0f); #else gx = (imu.readFloatGyroX() - biasGX) * (PI/180.0f); - (void)(imu.readFloatGyroY()); // GY unused — read to keep SPI/I2C sequence consistent + gy = (imu.readFloatGyroY() - biasGY) * (PI/180.0f); gz = (imu.readFloatGyroZ() - biasGZ) * (PI/180.0f); #endif @@ -347,12 +349,50 @@ void loop() { 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)); - // Pan (cursor X) = gyro Z (yaw). Nod (cursor Y) = gyro X (pitch). - // Confirmed by serial diagnostics: GZ fires on left/right pan, GX fires on up/down nod. - float fGx = (fabsf(gx) > cfg.deadZone) ? gx : 0.0f; - float fGz = (fabsf(gz) > cfg.deadZone) ? gz : 0.0f; + // ── Gravity-based axis decomposition ────────────────────────────────────── + // Low-pass filter accel to get a stable gravity estimate in device frame. + // This lets us project angular velocity onto world-aligned axes regardless + // of how the device is rolled. Device forward (pointing) axis = X. + // Confirmed by diagnostics: GX=roll, GY=nod, GZ=pan in user's hold. + const float GRAV_LP = 0.05f; + gravX += GRAV_LP * (ax - gravX); + gravY += GRAV_LP * (ay - gravY); + gravZ += GRAV_LP * (az - gravZ); - bool moving = (fGx != 0.0f || fGz != 0.0f); + 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; + + // Screen-right = cross(forward, up) = cross([1,0,0], [gnx,gny,gnz]) + // = [0, -gnz, gny] + 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). + // Use a wider dead zone for pitch to prevent constant cursor drift at rest. + float fYaw = (fabsf(yawRate) > cfg.deadZone) ? yawRate : 0.0f; + float fPitch = (fabsf(pitchRate) > cfg.deadZone * 3.0f) ? pitchRate : 0.0f; + + // DIAG: print every 500ms to debug gravity projection — remove when confirmed + #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(" 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); @@ -371,8 +411,8 @@ void loop() { accumX = accumY = 0.0f; flags |= 0x01; } else { - float rawX = applyAcceleration(applyCurve(-fGz * cfg.sensitivity * dt)); - float rawY = applyAcceleration(applyCurve(-fGx * cfg.sensitivity * dt)); + 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; accumX += rawX; accumY += rawY;