Fix a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint.

If certain inputs were passed to trajectory planner, an expression inside the trajectory planner which is an argument to sqrtf() could become negative due to finite floating point accuracy.
This led to Vr_ == NaN and then Tf_ == 0, causing the trajectory to jump to the final setpoint instantaneously. To the user, this manifested as a sudden increase in velocity (limited by controller.config.vel_limit) and/or an overcurrent fault.

This bug was likely to show up when constantly sending trajectory setpoints while moving in the negative direction.

See also: https://discourse.odriverobotics.com/t/move-to-pos-not-works-well/4626

Author: samuelsadok

CHANGELOG.md

@@ -2,6 +2,10 @@
 Please add a note of your changes below this heading if you make a Pull Request.
 
 # Releases
+## [0.4.12] - 2020-05-06
+### Changed
+* Fixed a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint
+
 ## [0.4.11] - 2019-07-25
 ### Added
 * Separate lockin configs for sensorless, index search, and general.

Firmware/MotorControl/trapTraj.cpp

@@ -44,7 +44,7 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
     // Are we displacing enough to reach cruising speed?
     if (s*dX < s*dXmin) {
         // Short move (triangle profile)
-        Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
+        Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
         Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
         Td_ = std::max(0.0f, -Vr_ / Dr_);
         Tv_ = 0.0f;