Fix roll correction, closes #1

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;

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++;

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).
+  // ── Gravity-based axis decomposition ──────────────────────────────────────
-  // Confirmed by serial diagnostics: GZ fires on left/right pan, GX fires on up/down nod.
+  // Low-pass filter accel to get a stable gravity estimate in device frame.
-  float fGx = (fabsf(gx) > cfg.deadZone) ? gx : 0.0f;
+  // This lets us project angular velocity onto world-aligned axes regardless
-  float fGz = (fabsf(gz) > cfg.deadZone) ? gz : 0.0f;
+  // 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 rawX = applyAcceleration(applyCurve(-fYaw   * cfg.sensitivity * dt));
-    float rawY = applyAcceleration(applyCurve(-fGx * 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;