nikrozman/air-mouse
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects 2 Releases Activity

Fix roll correction, closes #1

Browse Source
This commit is contained in:
Nik Rozman
2026-03-01 23:13:39 +01:00
parent b3cc319d5e
commit ee1d13f177
3 changed files with 53 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
source/main.cpp
View File

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