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

Simplify tracking

Browse Source
This commit is contained in:
Nik Rozman
2026-03-24 23:11:17 +01:00
parent 8ab07adfc6
commit 0fc38a5e1b
4 changed files with 35 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
source/config.h
View File

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

4
source/imu.cpp
View File

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

87
source/main.cpp
View File

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

13
source/sleep.cpp
View File

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