pbio/observer: Work around clamping bug.

See: pybricks/support#863

Author: laurensvalk

CHANGELOG.md

@@ -13,10 +13,12 @@
 - Fixed Essential hub hanging on boot when bootloader entered but USB cable
   not connected ([support#821]).
 - Fixed button needs debouncing on City/Technic/Essential hubs ([support#716]).
+- Fixed motor hold drifting away under external input movement ([support#863]).
 
 [support#716]: https://github.com/pybricks/support/issues/716
 [support#821]: https://github.com/pybricks/support/issues/821
 [support#849]: https://github.com/pybricks/support/issues/849
+[support#863]: https://github.com/pybricks/support/issues/863
 
 ## [3.2.0c1] - 2022-12-09
 

lib/pbio/src/observer.c

@@ -138,9 +138,16 @@ void pbio_observer_update(pbio_observer_t *obs, uint32_t time, const pbio_angle_
     obs->speed_numeric = pbio_differentiator_get_speed(&obs->differentiator, angle);
 
     // Apply observer error feedback as voltage.
-    int32_t feedback_voltage = pbio_observer_torque_to_voltage(m,
-        pbio_observer_get_feedback_torque(obs, angle)
-        );
+    // int32_t feedback_voltage = pbio_observer_torque_to_voltage(m,
+    //     pbio_observer_get_feedback_torque(obs, angle)
+    //     );
+    //
+    // HACK: The following implements the commented code above, but reorders the
+    //       multiplication and division steps to avoid clamping of an
+    //       intermediate result. This should be generalized when we revisit
+    //       the way the observer gain is defined.
+    //       See https://github.com/pybricks/support/issues/863
+    int32_t feedback_voltage = PRESCALE_TORQUE * obs->model->gain / m->d_voltage_d_torque * pbio_angle_diff_mdeg(angle, &obs->angle) / 1000;
 
     // Check stall condition.
     update_stall_state(obs, time, actuation, voltage, feedback_voltage);