Add hysteresis to prevent spurious acceleration sign changes

Add hysteresis to CalculateAcceleration() to prevent spurious
sign changes in the middle of the deceleration phase. Once
we start decelerating towards the target, continue decelerating
until we either overshoot (caught by the v_frame*dx<0 check)
or reach the target.

This eliminates mid-deceleration jitter caused by floating-point
noise in the distance-based threshold calculation. End-of-trajectory
oscillations (when velocity crosses zero) are still allowed as they
are part of normal overshoot correction.

The test expectations have been updated to:
- Distinguish mid-trajectory violations (should be 0) from
  end-of-trajectory oscillations (allowed up to 15)
- Allow slightly wider step range for trajectory completion
  (440-470 vs 440-460)
- Update expected durations for acceleration-only cases to
  account for the ~1% longer trajectory due to committing to
  deceleration earlier

Author: claude

fw/bldc_servo_position.h

@@ -87,6 +87,7 @@ class BldcServoPosition {
   }
 
   static float CalculateAcceleration(
+      const BldcServoStatus* status,
       BldcServoCommandData* data,
       float a,
       float v0,
@@ -112,7 +113,25 @@ class BldcServoPosition {
     if ((v_frame * dx) >= 0.0f && dx != 0.0f) {
       // The target is stationary and we are moving towards it.
       const float decel_distance = (v_frame * v_frame) / (2.0f * a);
-      if (std::abs(dx) >= decel_distance) {
+
+      // Compute what acceleration we would use based on distance.
+      const bool should_accelerate = std::abs(dx) >= decel_distance;
+
+      // Use hysteresis to prevent oscillation due to floating point
+      // precision issues.  Once we've started decelerating towards
+      // the target, continue decelerating until we either overshoot
+      // (caught by v_frame*dx<0 above) or reach the target.  This
+      // prevents numerical oscillation in the middle of the
+      // deceleration phase.
+      const float prev_accel = status->control_acceleration;
+      const bool was_decelerating_towards_target =
+          (prev_accel != 0.0f) && (prev_accel * v_frame < 0.0f);
+
+      if (was_decelerating_towards_target) {
+        return std::copysign(a, -v_frame);
+      }
+
+      if (should_accelerate) {
         if (std::isnan(data->velocity_limit) ||
             std::abs(v0) < data->velocity_limit) {
           return std::copysign(a, dx);
@@ -160,7 +179,7 @@ class BldcServoPosition {
     }
 
     const float acceleration = CalculateAcceleration(
-        data, a, v0, vf, dx, dv);
+        status, data, a, v0, vf, dx, dv);
 
     status->control_acceleration = acceleration;
     *status->control_velocity += acceleration * period_s;

fw/test/bldc_servo_position_test.cc

@@ -388,12 +388,14 @@ BOOST_AUTO_TEST_CASE(AccelVelocityLimits, * boost::unit_test::tolerance(1e-3)) {
 
     /////////////////////////////////
     // No velocity limit.
-    { 0.0,  0.0,    5.0, 0.0,   1.0, NaN, 40,   0.000, 4.514 },
-    { 0.0,  1.0,    5.0, 0.0,   1.0, NaN, 40,   0.000, 3.730 },
-    { 0.0,  1.0,    5.0, 1.5,   1.0, NaN, 40,   0.000, 5.025 },
-    { 0.0, -1.0,   -5.0,-1.5,   1.0, NaN, 40,   0.000, 5.025 },
-    { 5.0,  0.0,    0.0, 0.0,   1.0, NaN, 40,   0.000, 4.514 },
-    { 5.0,  0.0,    0.0, 0.0,   2.0, NaN, 40,   0.000, 3.205 },
+    // Note: expected durations include ~1% increase due to hysteresis
+    // in acceleration calculations (see CalculateAcceleration).
+    { 0.0,  0.0,    5.0, 0.0,   1.0, NaN, 40,   0.000, 4.568 },
+    { 0.0,  1.0,    5.0, 0.0,   1.0, NaN, 40,   0.000, 3.791 },
+    { 0.0,  1.0,    5.0, 1.5,   1.0, NaN, 40,   0.000, 5.140 },
+    { 0.0, -1.0,   -5.0,-1.5,   1.0, NaN, 40,   0.000, 5.141 },
+    { 5.0,  0.0,    0.0, 0.0,   1.0, NaN, 40,   0.000, 4.568 },
+    { 5.0,  0.0,    0.0, 0.0,   2.0, NaN, 40,   0.000, 3.212 },
 
     /////////////////////////////////
     // Accel and velocity limits
@@ -653,6 +655,7 @@ BOOST_AUTO_TEST_CASE(ControlAccelerationConsistent) {
   // strictly negative, and then 0, with no other transitions.
 
   int negative_violations = 0;
+  int end_of_trajectory_violations = 0;
   int zero_violations = 0;
 
   constexpr int kMaxSteps = 50000;
@@ -686,7 +689,17 @@ BOOST_AUTO_TEST_CASE(ControlAccelerationConsistent) {
             expected_accel = kZero;
             break;
           } else {
-            negative_violations++;
+            // Track violations separately for mid-trajectory vs end-of-trajectory.
+            // End-of-trajectory oscillations (when velocity is small) are expected
+            // as part of normal overshoot correction.  Mid-trajectory violations
+            // (when velocity is significant) should not occur with hysteresis.
+            const float vel = ctx.status.control_velocity.value();
+            const float vel_threshold = kAccel * (1.0f / ctx.rate_hz) * 50.0f;
+            if (std::abs(vel) > vel_threshold) {
+              negative_violations++;
+            } else {
+              end_of_trajectory_violations++;
+            }
           }
           break;
         }
@@ -700,12 +713,15 @@ BOOST_AUTO_TEST_CASE(ControlAccelerationConsistent) {
     }
   }
 
+  // With hysteresis, trajectories may take slightly longer (about 1-2%)
+  // due to committing to deceleration once started.
   BOOST_TEST(steps_to_complete >= 440);
-  BOOST_TEST(steps_to_complete <= 460);
-
-  // Because of floating point precision issues in the current
-  // implementation, we aren't perfect.  Still, verify that it
-  // *mostly* does what we want.
-  BOOST_TEST(negative_violations <= 15);
-  BOOST_TEST(zero_violations <= 2);
+  BOOST_TEST(steps_to_complete <= 470);
+
+  // With hysteresis in the acceleration calculation, we should have
+  // no spurious mid-trajectory sign changes.  End-of-trajectory
+  // oscillations are expected as part of normal overshoot correction.
+  BOOST_TEST(negative_violations == 0);
+  BOOST_TEST(end_of_trajectory_violations <= 15);
+  BOOST_TEST(zero_violations == 0);
 }