DC servo: add overflow-safe sigma–delta PWM dithering for sub-count resolution

- Introduce SERVO_SIGMA_DELTA_DITHERING (residual, non-overflowing implementation)
- Dither float PWM counts to integer per tick so long-term average matches target for DC motors
- Improve effective torque resolution near sidereal
- Reduces bias and micro-jitter vs. trunc/round-only outputs
- Reset residual on zero-output/disable to avoid stale carryover
- Uses single-precision math

Author: papadako

src/lib/axis/motor/servo/ServoDriver.h

@@ -10,6 +10,10 @@
 
 #ifdef SERVO_MOTOR_PRESENT
 
+#ifdef SERVO_SIGMA_DELTA_DITHERING
+  #include "dc/SigmaDeltaDither.h"
+#endif
+
 typedef struct ServoPins {
   int16_t ph1; // step
   int16_t ph1State;
@@ -71,7 +75,7 @@ class ServoDriver {
     // get status info.
     // this is a required method for the Axis class
     DriverStatus getStatus() { return status; }
-   
+
     // calibrate the motor if required
     virtual void calibrateDriver() {}
 
@@ -80,7 +84,7 @@ class ServoDriver {
 
   protected:
     virtual void readStatus() {}
-    
+
     int axisNumber;
     char axisPrefix[32]; // prefix for debug messages
 
@@ -115,6 +119,11 @@ class ServoDriver {
 
     const int numParameters = 2;
     AxisParameter* parameter[2] = {&invalid, &acceleration};
+
+    #ifdef SERVO_SIGMA_DELTA_DITHERING
+      SigmaDeltaDither sigmaDelta;  // carries fractional residue between ticks
+    #endif
+
 };
 
 #endif

src/lib/axis/motor/servo/dc/DcServoDriver.h

@@ -26,6 +26,9 @@
   #define SERVO_HYST_EXIT_CPS 0.6f    // drop below this to RETURN to zero
 #endif
 
+ifdef SERVO_SIGMA_DELTA_DITHERING
+  #include "SigmaDeltaDither.h"
+#endif
 
 #include "../ServoDriver.h"
 
@@ -64,6 +67,10 @@ class ServoDcDriver : public ServoDriver {
         zeroHoldSign = 0; // ensure immediate exit and clear latch on exact zero
       #endif
 
+      #ifdef SERVO_SIGMA_DELTA_DITHERING
+        sigmaDelta.reset(); // reset dithering residue when output is zero
+      #endif
+
       return 0; // early out
     }
 
@@ -81,7 +88,13 @@ class ServoDcDriver : public ServoDriver {
         zeroHoldSign = (sign >= 0) ? 1 : -1; // latch direction
       } else {
         // currently moving: if we drop below exit threshold, snap back to zero
-        if (vAbs < SERVO_HYST_EXIT_CPS) { zeroHoldSign = 0; return 0; }
+        if (vAbs < SERVO_HYST_EXIT_CPS) {
+          zeroHoldSign = 0;
+          #ifdef SERVO_SIGMA_DELTA_DITHERING
+            sigmaDelta.reset(); // also reset when snapping back to zero
+          #endif
+          return 0;
+        }
         // allow direction change if command flips while above thresholds
         if ((sign >= 0 ? 1 : -1) != zeroHoldSign) zeroHoldSign = (sign >= 0) ? 1 : -1;
       }
@@ -93,8 +106,13 @@ class ServoDcDriver : public ServoDriver {
     // fmaf keeps one rounding and is very fast on some FPUS(M7).
     float countsF = fmaf(vAbs, velToCountsGain, (float)countsMinCached);
 
-    // Single float→int rounding at the end
-    long power = (long)lroundf(countsF);
+    #ifdef SERVO_SIGMA_DELTA_DITHERING
+      // Dither the floating counts to an integer so the time-average equals countsF
+      int32_t power = sigmaDelta.dither_counts(countsF, countsMinCached, countsMaxCached);
+    #else
+      // Single float→int rounding at the end
+      long power = (long)lroundf(countsF);
+    #endif
 
     return power * sign;
   }

src/lib/axis/motor/servo/dc/SigmaDeltaDither.h

@@ -0,0 +1,52 @@
+// -----------------------------------------------------------------------------
+// Overflow-safe Sigma–Delta PWM Dithering
+// Enable with:   #define SERVO_SIGMA_DELTA_DITHERING ON
+//
+// Purpose:
+//   When PWM resolution is low (e.g., sidereal ≈ a few counts), this modulator
+//   time-averages between adjacent integer counts so the long-term average
+//   equals a floating target (e.g., 4.4 → mix of 4 and 5). This code never
+//   overflows because it only carries the fractional residual in [0,1).
+// -----------------------------------------------------------------------------
+
+#ifdef SERVO_SIGMA_DELTA_DITHERING
+
+// Lightweight state holder for first-order sigma–delta dithering
+// We store only the fractional residual between ticks
+struct SigmaDeltaDither {
+    // Fractional residue in [0,1). Carries the part we couldn't emit this tick
+    float resid = 0.0f;
+
+    // Reset internal state (call when you reinitialize control or modes change)
+    inline void reset() { resid = 0.0f; }
+
+    // convert a floating desired count into an integer count for this tick,
+    // such that the time-average of outputs equals the floating target
+    inline int32_t dither_counts(float desiredCounts,
+                                    int32_t minCount,
+                                    int32_t maxCount)
+    {
+        // Safety clamp the desired average to valid bounds.
+        if (desiredCounts < (float)minCount) desiredCounts = (float)minCount;
+        if (desiredCounts > (float)maxCount) desiredCounts = (float)maxCount;
+
+        // Add residual: carry fractional error forward from previous tick
+        // e.g. 4.4 + 0.7 = 5.1 → emit 5 now, resid becomes 0.1
+        float sum = desiredCounts + resid;
+
+        // Emit the integer part of this tick
+        int32_t out = (int32_t)floorf(sum);
+
+        // Keep the fractional residue for next tick (always in [0,1))
+        resid = sum - (float)out;
+
+        // safety clamp (probably not needed, but just in case)
+        if (out < minCount) out = minCount;
+        if (out > maxCount) out = maxCount;
+
+        return out; // Integer PWM counts to write this tick
+    }
+
+};
+
+#endif