diff --git a/source/main.cpp b/source/main.cpp index 250c267..8791ff3 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -59,6 +59,8 @@ TelemetryPacket telem = {}; // Tuning constants const float ALPHA = 0.96f; 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 int BIAS_SAMPLES = 200; const int IDLE_FRAMES = 150; const unsigned long BATT_REPORT_MS = 20000; @@ -425,11 +427,15 @@ void loop() { int8_t moveX = 0, moveY = 0; uint8_t flags = 0; + static float smoothX = 0.0f, smoothY = 0.0f; + if (shocked) { // Shock freeze - discard accumulated sub-pixel motion and suppress output + smoothX = smoothY = 0.0f; accumX = accumY = 0.0f; flags |= 0x08; // bit3 = shock freeze active } else if (idle) { + smoothX = smoothY = 0.0f; accumX = accumY = 0.0f; flags |= 0x01; } else { @@ -437,7 +443,14 @@ void loop() { float rawY = applyAcceleration(applyCurve(-fPitch * cfg.sensitivity * dt)); if (cfg.axisFlip & 0x01) rawX = -rawX; if (cfg.axisFlip & 0x02) rawY = -rawY; - accumX += rawX; accumY += 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; + accumX += smoothX; accumY += smoothY; moveX = (int8_t)constrain((int)accumX, -127, 127); moveY = (int8_t)constrain((int)accumY, -127, 127); accumX -= moveX; accumY -= moveY;