Simplify tracking
This commit is contained in:
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user