Better smoothing

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;